Ask Your Question
0

Rospy isn't catching KeyboardInterrupt on the second iteration of the loop

asked 2017-05-25 19:36:43 -0600

Mav16 gravatar image

updated 2017-05-25 20:50:55 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

Why are you creating a publisher and starting a node within your command function? This means that every time this function is called a publisher will be created and the node will be invited. I have a feeling that this may be part of the issue.

jayess gravatar imagejayess ( 2017-05-25 19:55:51 -0600 )edit

Try going back to the tutorials and see if you get to the root of the problem.

jayess gravatar imagejayess ( 2017-05-25 19:56:23 -0600 )edit

I modified the code and moved the node and publisher out of that function. The error persists. It stops catching KeyboardInterrupt as soon as the node is created.

Mav16 gravatar imageMav16 ( 2017-05-25 20:35:34 -0600 )edit

What versions of ROS and (I presume) Ubuntu are you using? How are you running your node? E.g., using rosrun, roslaunch, or some other way? What's ROS telling you? When you say you changed the code, what did you do? Can you post it? The more information you give the better help you can get.

jayess gravatar imagejayess ( 2017-05-25 20:40:34 -0600 )edit
1

This probably isn't your issue, but your implementation of the rospy.Rate.sleep function is incorrect. If you want to sleep for 0.1 seconds, use rospy.sleep(0.1). The use of rospy.Rate is controlling the rate of loops. There should only ever be one rospy.Rate.sleep() per loop, at the end of it

ufr3c_tjc gravatar imageufr3c_tjc ( 2017-05-25 20:44:03 -0600 )edit

@jayess, I'm sorry, I edited the main question with the updated code. I'm using Indigo on Ubuntu 14.04. I'm running it like any other python script using python

Mav16 gravatar imageMav16 ( 2017-05-25 20:44:48 -0600 )edit

What are your inputs? When I run your edited code I keep getting ValueError: math domain error with values between 0.0001 to 5.

jayess gravatar imagejayess ( 2017-05-25 23:25:01 -0600 )edit

@jayess, I haven't done a full workspace analysis on this but for now X=0.3 and Y = 0.1 should work.

Mav16 gravatar imageMav16 ( 2017-05-25 23:37:23 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2017-05-25 20:57:33 -0600

ufr3c_tjc gravatar image

updated 2017-05-25 23:45:00 -0600

Now that I think about it, your incorrect use of rospy.Rate.sleep() may be the cause. So try this:

  • turn both of the rate.sleep() calls into rospy.sleep(0.1) calls
  • change the loop rate to be 0.5 seconds per loop, like rate = rospy.Rate(2)
  • put rate.sleep() at the very end of your while loop, after the try/except block and in-line with it

See if that changes anything. Sleep calls listen for Ctrl+C interrupts. Your loop timing will be all out of whack due to using a loop sleep twice, which may be causing the sleeps to not catch interrupts.

EDIT: Actually, let me alter that a little bit. You don't want a rospy.Rate.sleep() call at all here, because its loop timing will be dependent on user input (not good). So just use another rospy.sleep(1) or something at the end of your loop to catch Ctrl+C interrupts.

EDIT 2 (answer): The problem may lie in the fact that ROS is handling signals itself, and so a KeyboardInterrupt is never actually raised. Try setting disable_signals=True in your node initialization, like rospy.init_node('commander', anonymous=True, disable_signals=True). Disabling signals won't cause any major issues. ROS just uses SIGINT to shut down nodes itself, preventing users to have to explicitly look for and catch KeyboardInterrupt exceptions etc. You're basically doing the same thing yourself.

edit flag offensive delete link more

Comments

Nope, that didn't fix the issue.

Mav16 gravatar imageMav16 ( 2017-05-25 21:09:11 -0600 )edit

When are you attempting to interrupt it? When it is requesting data, or when it is running afterwards?

ufr3c_tjc gravatar imageufr3c_tjc ( 2017-05-25 21:11:39 -0600 )edit

The problem may lie in the fact that ROS is handling signals itself, and so a KeyboardInterrupt is never actually raised. Try setting disable_signals=True in your node initialization, like rospy.init_node('commander', anonymous=True, disable_signals=True)

ufr3c_tjc gravatar imageufr3c_tjc ( 2017-05-25 21:31:22 -0600 )edit
1

Yep. This worked. What repercussions will disabling signals have? If you could just edit the main response, I'll mark it as the answer.

Mav16 gravatar imageMav16 ( 2017-05-25 23:36:41 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-05-25 19:36:43 -0600

Seen: 570 times

Last updated: May 25 '17