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

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
th2 = angles
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
th2 = angles
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 ...
edit retag close merge delete

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.

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

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.

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.

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

@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

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, I haven't done a full workspace analysis on this but for now X=0.3 and Y = 0.1 should work.

Sort by » oldest newest most voted 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.

more

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

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)

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.