Return to Answer

• 1

• 13.5k
• 5
• 37
• 62

• 29.8k
• 1
• 49
• 93

A few obvious problems :

• global variables (SuperBiaisedMan cover the topic pretty well, kudos!)

• code repetition : everything is done so many times that it will be a pain if you have to change anything. One can see in the commented code that you suffered a bit of it. Copy-pasting should not be your best friend.

• code organisation : everything is all over the place. It is impossible to tell which piece of datas are linked to the same thing.

To solve most of these issues, our friend will be functions and classes which help you split your code into logical pieces that can be easily reused.

The pieces of code below might need adjustments based on your need but should give you a few hints. Also the vocabulary I used might be wrong because I don't understand everything.

Joints

One thing I realized first is that you have 18 publishers which will be used to publish 18 positions. Also, we have 6 suscribers created with 6 callbacks updating 6 flags based on 6 (out of the 18 positions). My first thought was that I would be nice to a object to handle that kind of logic.

I've defined a new class like this :

class Joint():
 def __init__(self, name, position=0.0):
 self.name = name
 self.position = position
 self.publisher = rospy.Publisher('/' + name + '/command', Float64, latch=True)
 self.flag = 1
 def publish(self):
 self.publisher.publish(self.position) 
 
 def subscribe(self):
 def callback(msg):
 self.flag = 1 if abs(msg.current_pos - self.position) < delta else 0
 rospy.Subscriber('/' + name + '/state', JointState, callback)

On your Joint object, you can call subscribe() or publish() and it should just work. You might want to add other attributes like side (left/right), position (front, middle, back), index (1, 2, 3) but I had troubles as it was conflicting with the already created position attribute.

Then I thought : "do you know who is good at handling Joints? A robot!" and created another class which might or might not be useful to you.

class Robot():
 def __init__(self):
 self.moving = 1
 self.joints = []
 def send_command(self):
 for j in self.joints:
 j.publish()
 def subscribe(self):
 for j in self.joints:
 if j.is_middle: # to be defined
 j.subscribe
r = Robot()
for side in 'LR':
 for pos in 'FRM':
 for num in '123':
 r.joints.add(Joint(side + pos + num))

Functions to compute desired position

Another great tool to write reusable code is simple function. Sometimes, the hard part is to detect what is similar enough to be extracted out in functions. For most of your code, I had no idea what was going on so there is little I can do but by the end I noticed you were doing repeated calculations. At the end of the day, the 60 lines of computations were doing simple things : changing position based on some state. These computations, mostly repeated for left and right side are pretty similar each time, only a few constants are different based on the state we are in.

Thus, I wrote a function to retrieve the constants based on the state. Call it for the left side, then for the right side and then update positions accordingly.

def get_pos_for_state(state)
 if state == 1:
 return P_j1_s1, P_j2_s1, P_j3_s1
 elif state == 2:
 return P_j1_s2, P_j2_s2, P_j3_s2
 elif state == 3:
 return P_j1_s3, P_j2_s3, P_j3_s3
 assert False, "State must be 1, 2 or 3"
l1, l2, l3 = get_pos_for_state(LM_state)
r1, r2, r3 = get_pos_for_state(RM_state)
p_LM1 = Ra_L * l1
p_RM1 = - Ra_R * r1
p_LM2 = l2
p_RM2 = - r2
p_LM3 = l3
p_RM3 = - r3
p_RR1 = p_RF1 = - Ra_R * l1
p_LR1 = p_LF1 = Ra_L * r1
p_RR2 = p_RF2 = - l2
p_LR2 = p_LF2 = r2
p_RR3 = p_RF3 = - l3
p_LR3 = p_LF3 = r3

Even without knowing what this does, the symetry in the code is aesthetically pleasant and I hope it is correct :-).

I hope this will help you with your code... and your robot.