arm_controller.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('pr2_precise_trajectory')
00002 import rospy
00003 from trajectory_msgs.msg import JointTrajectory,JointTrajectoryPoint
00004 from pr2_controllers_msgs.msg import JointTrajectoryGoal, JointTrajectoryAction
00005 from actionlib import SimpleActionClient, SimpleGoalState
00006 import trajectory_msgs.msg
00007 
00008 ARM_JOINTS = ["shoulder_pan_joint", "shoulder_lift_joint", "upper_arm_roll_joint",
00009              "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
00010 
00011 def get_arm_joint_names(side):
00012     return [ '%s_%s' % (side, name) for name in ARM_JOINTS ]
00013 
00014 class ArmController:
00015     def __init__(self, arm):
00016         self.client = SimpleActionClient("%s_arm_controller/joint_trajectory_action"%arm, JointTrajectoryAction)
00017         #wait for the action servers to come up 
00018         rospy.loginfo("[ARM] Waiting for %s controller"%arm)
00019         self.client.wait_for_server()
00020         rospy.loginfo("[ARM] Got %s controller"%arm)
00021 
00022     def start_trajectory(self, trajectory, set_time_stamp=True, wait=True):
00023         """Creates an action from the trajectory and sends it to the server"""
00024         goal = JointTrajectoryGoal()
00025         goal.trajectory = trajectory
00026         if set_time_stamp:
00027             goal.trajectory.header.stamp = rospy.Time.now()
00028         self.client.send_goal(goal)
00029 
00030         if wait:
00031             self.wait()
00032 
00033     def wait(self):
00034         self.client.wait_for_result()
00035 
00036     def is_done(self):
00037         return self.client.get_state() > SimpleGoalState.ACTIVE
00038 


pr2_precise_trajectory
Author(s): David V. Lu!!
autogenerated on Fri Aug 28 2015 12:12:32