Issue with multithreading and standard output
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 ...