0
$\begingroup$

Rosanswers logo

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

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

It looks like you're accidentally sending some kind of control code to the terminal subsystem that's disabling the local echo and carriage returns.

For starters, you should be able to reset your terminal to the default state with the stty sane command (hit enter, type and pray).

Beyond that, I would inspect your program's print statements for stray escape sequences that could be causing this. If you don't find anything obvious there, you could try piping the output through something like hexdump to decode all of the console output before is passes into the pseudo-tty layer and messes things up.

Go read "Advanced Programming in the UNIX Environment" or another similar UNIX systems programming book for more details of the underlying mechanics here.


Originally posted by ahendrix with karma: 47576 on 2014-02-10

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by rse101 on 2014-02-11:
Thanks for the help. stty sane worked beautifully. I don't really have time at the moment to parse through the output and figure out exactly what is happening, so for the moment I just hacked it and added 'call(["stty", "sane"])' in the last if block right after 't.join()'. It works for now, I'll fix it later. Thanks again!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.