Go to the documentation of this file.00001
00002
00003
00004
00005
00006 import roslib
00007 roslib.load_manifest('coverage_tools')
00008 import rospy
00009 from coverage_tools.srv import *
00010 from sensor_msgs.msg import JointState
00011 from pr2_controllers_msgs.msg import *
00012 from trajectory_msgs.msg import *
00013 import threading
00014 import actionlib
00015
00016 from coverage_tools.srv import ReturnJointStates
00017 import time
00018 import sys
00019
00020 def get_joint_states(joint_names):
00021 rospy.wait_for_service("return_joint_states")
00022 try:
00023 s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
00024 resp = s(joint_names)
00025 except rospy.ServiceException, e:
00026 print "error when calling return_joint_states: %s"%e
00027 sys.exit(1)
00028 for (ind, joint_name) in enumerate(joint_names):
00029 if(not resp.found[ind]):
00030 print "joint %s not found!"%joint_name
00031 return resp.position
00032
00033 def stop_joint_trajectory():
00034 client = actionlib.SimpleActionClient("r_arm_controller/joint_trajectory_action", pr2_controllers_msgs.msg.JointTrajectoryAction)
00035 client.wait_for_server()
00036 joint_names= ["r_shoulder_pan_joint", "r_shoulder_lift_joint","r_upper_arm_roll_joint","r_elbow_flex_joint","r_forearm_roll_joint","r_wrist_flex_joint","r_wrist_roll_joint"]
00037
00038 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00039 goal.trajectory.joint_names = joint_names
00040
00041 traj_point = trajectory_msgs.msg.JointTrajectoryPoint()
00042 traj_point.time_from_start = rospy.Duration.from_sec(1.0)
00043 for i in range(0,7):
00044 traj_point.velocities.append(0)
00045 pos_list = get_joint_states(joint_names)
00046 print list(pos_list)
00047 traj_point.positions = list(pos_list)
00048
00049 print "Calling client to stop trajectory"
00050 client.send_goal(goal)
00051 client.wait_for_result()
00052 return client.get_result()
00053
00054
00055 if __name__ == "__main__":
00056
00057 rospy.init_node('trajectory_stopper')
00058 print "trying to stop trajectory"
00059 print stop_joint_trajectory()
00060 rospy.spin()