head_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 HEAD_JOINTS = ['head_pan_joint', 'head_tilt_joint']
00009 
00010 class HeadController:
00011     def __init__(self):
00012         self.client = SimpleActionClient('/head_traj_controller/joint_trajectory_action', JointTrajectoryAction)
00013         #wait for the action servers to come up 
00014         rospy.loginfo("[HEAD] Waiting for controller")
00015         self.client.wait_for_server()
00016         rospy.loginfo("[HEAD] Got controller")
00017 
00018     def start_trajectory(self, trajectory, set_time_stamp=True, wait=True):
00019         """Creates an action from the trajectory and sends it to the server"""
00020         goal = JointTrajectoryGoal()
00021         goal.trajectory = trajectory
00022         if set_time_stamp:
00023             goal.trajectory.header.stamp = rospy.Time.now()
00024         self.client.send_goal(goal)
00025 
00026         if wait:
00027             self.wait()
00028 
00029     def wait(self):
00030         self.client.wait_for_result()
00031 
00032     def is_done(self):
00033         return self.client.get_state() > SimpleGoalState.ACTIVE
00034 


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