Mav16's profile - activity

I've written a publisher in Python to publish joint angles to a simple robot. The program only catches KeyboardInterrupt on the very first loop before I've ever published anything. However, as soon as I enter the X&Y values and the code does its thing, it stops responding to KeyboardInterrupt. The programs doesn't publish after I press Ctrl+C but it doesn't quit either. I'm quite stumped, any help would be appreciated. TIA

 #!/usr/bin/env python
import rospy
import math
from std_msgs.msg import Float64
import time


def main():
    while not rospy.is_shutdown():
        try:
            #~ joint = raw_input('Enter joint: ')
            #~ angle =2 float(raw_input('Enter angle in radians: '))
            #~ command(joint,angle)
            x = float(raw_input("Enter end effector X in meters: "))
            y = float(raw_input("Enter end effector Y in meters: "))
            angles = ik(x,y)
            th1 = angles[0]
            th2 = angles[1]
            print('Angles - Shoulder: %f, Elbow: %f' %(th1,th2))
            command('Shoulder',th1*-1)
            command('Elbow',th2*-1)
        except KeyboardInterrupt:
            print('Process shutting down')
            break


def ik(x,y):
    l1 = .390
    l2 = .090

    B = math.sqrt(math.pow(x,2)+math.pow(y,2))
    q1 = math.atan2(y,x)
    q2 = math.acos((math.pow(l1,2)-math.pow(l2,2)+math.pow(B,2))/(2*l1*B))
    th1 = q1+q2
    th2 = math.acos((math.pow(l1,2)+math.pow(l2,2)-math.pow(B,2))/(2*l1*l2))
    return [th1,th2]

def command(joint,angle):
    topic = ('camera_arm/%s_position_controller/command' %joint)
    print topic
    pub = rospy.Publisher(topic,Float64,queue_size=1,latch=True)
    rospy.init_node('commander', anonymous=True)
    rate = rospy.Rate(10)
    if not rospy.is_shutdown():
        rospy.loginfo('Publishing angle %f to %s joint'% (angle, joint))
        pub.publish(angle)
        rate.sleep()
    return 0


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

Edit: jayess suggested that my repeated publisher creation might be an issue so I modified the code as follows but the error persists.

#!/usr/bin/env python
import rospy
import math
from std_msgs.msg import Float64
import time


def main():
    rospy.init_node('commander', anonymous=True)
    rate = rospy.Rate(10)
    shoulder  = rospy.Publisher("camera_arm/Shoulder_position_controller/command",Float64,queue_size=1,latch=True)
    elbow = rospy.Publisher("camera_arm/Elbow_position_controller/command",Float64,queue_size=1,latch=True)
    while not rospy.is_shutdown():
        try:
            x = float(raw_input("Enter end effector X in meters: "))
            y = float(raw_input("Enter end effector Y in meters: "))
            angles = ik(x,y)
            th1 = angles[0]
            th2 = angles[1]
            print('Angles - Shoulder: %f, Elbow: %f' %(th1,th2))

            if not rospy.is_shutdown():
                rospy.loginfo('Publishing angle %f to Shoulder joint'% (th1))
                shoulder.publish(th1)
                rate.sleep()
                rospy.loginfo('Publishing angle %f to Elbow joint'% (th2))
                elbow.publish(th2)
                rate.sleep()

        except KeyboardInterrupt:
            print('Process shutting down')
            break


def ik(x,y):
    l1 = .390
    l2 = .090

    B = math.sqrt(math.pow(x,2)+math.pow(y,2))
    q1 = math.atan2(y,x)
    q2 = math.acos((math.pow(l1,2)-math.pow(l2,2)+math.pow(B,2))/(2*l1*B))
    th1 = q1+q2
    th2 = math.acos((math.pow(l1,2)+math.pow(l2,2)-math.pow ...