I have a multithreaded piece of python code that moves the arms of my robot. The basic premise is that there is one thread for each arm, and threads block until each arm as finished it's movements (i.e., given two sets of joint positions, one for each arm, the program splits into two threads that loop until their respective arm has reached the specified joint positions). Here is the code that does that:
def __moveToJointPos(self, instance, index): if instance >= self.numInstances(): raise Exception("Called with instance " + str(instance) + " but only " + str(self.numInstances()) + " instances exist")
t = None
if type(index) is tuple:
#If this method is being called with 2 indices rather than one, that means we want to move both arms at the same time.
#To do this, we spin off one of the indices to another thread, then execute as normal on the remaining one.
#Then we wait until the other thread finishes before returning.
if len(index) > 2:
raise Exception("Can only call with a tuple of size 2, got a tuple of size " + str(len(index)))
elif self.poss[instance][1][index[0]][0] == self.poss[instance][1][index[1]][0]:
raise Exception("When calling with a tuple, must specify indices with different arms.")
elif len(index) == 2:
i2 = index[1]
t = threading.Thread(target=self._Behavior__moveToJointPos, args = (instance, i2))
t.daemon = True
t.start()
mov = self.poss[instance][1][index[0]]
else:
mov = self.poss[instance][1][index]
rate = rospy.Rate(10)
if mov[0] == Behavior.RIGHT_ARM:
arm = self.right_arm
else:
arm = self.left_arm
done = False
dd = [9999.0, 9999.0, 9999.0, 9999.0, 9999.0]
while not done:
arm.set_joint_positions(mov[1])
dist = 0.0
for j in arm.joint_names():
dist = dist + (mov[1][j] - arm.joint_angle(j))*(mov[1][j] - arm.joint_angle(j))
dist = math.sqrt(dist)
ss = 0.0
for d in dd:
ss = ss + math.fabs(d - dist)
if ss < 0.01:
done = True
elif rospy.is_shutdown():
print("ROS shutdown, cancelled movement")
done = True
if baxter_external_devices.getch() in ['\x1b', '\x03']:
print("stopped")
done = True
else:
dd.append(dist)
del dd[0]
rate.sleep()
if t is not None:
t.join()
I'll admit the code is rather messy, but this is just something I'm quickly hacking together to test out the robot. Here is the code that calls that function:
print("a")
self._Behavior__moveToJointPos(0,(ToTable.lHOME, ToTable.rHOME))
print("b")
self._Behavior__moveToJointPos(0,(ToTable.lMID, ToTable.rMID))
print("c")
self._Behavior__moveToJointPos(0,(ToTable.lTABLE, ToTable.rTABLE))
print("d")
(the prints are added so that the effect on standard out is more clear). Here is what my terminal looks like:
Here is the link to the image: http://i.imgur.com/1hdCq7f.png
(you may need to open it in a separate tab to see it). There are a couple more calls to __moveToJointPos, which is why there is more text output, but the error starts with the set of commands above. As you can see, after "b" is printed, each successive line starts indented over as far as the end of the line before it. This trend continues through all the print statements, and even after the program exits, my terminal no longer works correctly. Pressing enter, which normally starts a new line, instead just prints the terminal prompt right after the prompt that is already there. Additionally, the terminal does not echo any of my key presses back to me. The only way I can find to fix this is to close the terminal and open a new one. This is not a ton of fun to have to do each time I run my program.
I don't really have any idea what is going on here. If I change the calls to __moveToJointPos such that index is no longer a tuple (and thus that function does not spin off a helper thread), then everything works just fine. The error with standard out only occurs when there is multithreading involved. It's worthwhile to note that there are no calls to print while the program is running multiple threads, only before and after, so I'm not really sure what is happening. What am I doing wrong and does anyone know how to fix it?
Thank you
Originally posted by rse101 on ROS Answers with karma: 36 on 2014-02-10
Post score: 0