moveit_canceler.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import os
00005 import roslib
00006 roslib.load_manifest("denso_pendant_publisher")
00007 roslib.load_manifest("actionlib_msgs")
00008 
00009 import denso_pendant_publisher.msg
00010 import std_msgs.msg
00011 import actionlib_msgs.msg
00012 
00013 rospy.init_node("moveit_canceler")
00014 
00015 g_runnable = True
00016 
00017 g_prev_status = None
00018 
00019 
00020 def pendantCB(msg):
00021     global g_runnable, g_prev_status
00022     if g_prev_status:
00023         if (not g_prev_status.button_cancel and msg.button_cancel) or (not g_prev_status.button_stop and msg.button_stop): # canceled or stopped
00024             g_runnable = False
00025             # here we should send cancel
00026             cancel = actionlib_msgs.msg.GoalID()
00027             cancel.id = ""
00028             cancel_pub.publish(cancel)
00029             rospy.loginfo("cancel")
00030     g_prev_status = msg
00031 
00032 
00033 sub = rospy.Subscriber("/denso_pendant_publisher/status", denso_pendant_publisher.msg.PendantStatus, pendantCB)
00034 cancel_pub = rospy.Publisher("/arm_controller/follow_joint_trajectory/cancel", actionlib_msgs.msg.GoalID);
00035 # cancel_pub = rospy.Publisher("/move_group/cancel", actionlib_msgs.msg.GoalID);
00036 rospy.spin()


vs060
Author(s): Ryohei Ueda
autogenerated on Wed Aug 26 2015 11:21:15