stop_joint_trajectory.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #spins off a thread to listen for joint_states messages
00004 #and provides the same information (or subsets of) as a service
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()  # A FibonacciResult
00053 
00054 #run the server
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()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


coverage_3d_tools
Author(s): Juergen Hess
autogenerated on Wed Dec 26 2012 15:25:37