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[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 ...
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.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, userospy.sleep(0.1)
. The use of rospy.Rate is controlling the rate of loops. There should only ever be onerospy.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.