base_controller.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('pr2_precise_trajectory')
00002 import rospy
00003 import tf
00004 import actionlib
00005 from  geometry_msgs.msg import Twist, PoseStamped
00006 from tf.transformations import euler_from_quaternion
00007 from pr2_precise_trajectory.msg import *
00008 from math import copysign
00009 
00010 def orientation_to_euler(orientation):
00011     return euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])
00012 
00013 ACTION_NAME = '/base_controller/move_sequence'
00014 
00015 class BaseController:
00016     def __init__(self):
00017         self.cmd_pub = rospy.Publisher('/base_controller/command', Twist)
00018         self.tf = tf.TransformListener()
00019         self.server = actionlib.SimpleActionServer(ACTION_NAME, MoveSequenceAction, self.execute, False) 
00020         self.server.start()
00021 
00022         self.client = actionlib.SimpleActionClient(ACTION_NAME, MoveSequenceAction)
00023         rospy.loginfo("[BASE] Waiting for %s..."%ACTION_NAME)
00024         self.client.wait_for_server()
00025 
00026         self.frame = rospy.get_param('frame_id', '/base_footprint')
00027         self.tlim = rospy.get_param('translation_speed_limit', 1.2)
00028         self.rlim = rospy.get_param('rotation_speed_limit', 1.4)
00029 
00030         rospy.loginfo("[BASE] Ready!")
00031 
00032     def send_goal(self, goal):
00033         self.client.send_goal(goal)
00034 
00035     def wait_for_goal(self):
00036         self.client.wait_for_result()
00037 
00038     def execute(self, goal):
00039         r = rospy.Rate(100)
00040 
00041         # wait to start
00042         while rospy.Time.now() < goal.header.stamp:
00043             r.sleep()
00044 
00045         feedback = MoveSequenceFeedback()
00046         t_prev = goal.header.stamp
00047         for pose, time in zip(goal.poses, goal.times):
00048             goal_time = t_prev + rospy.Duration(time)
00049             t_prev = goal_time
00050             goal_pose = PoseStamped()
00051             goal_pose.header.frame_id = goal.header.frame_id
00052             goal_pose.pose = pose
00053 
00054             while rospy.Time.now() <= goal_time:
00055                 try:
00056                     relative = self.tf.transformPose(self.frame, goal_pose)
00057                 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00058                     continue
00059 
00060                 dx = relative.pose.position.x
00061                 dy = relative.pose.position.y
00062                 rot = orientation_to_euler(relative.pose.orientation)
00063                 dz = rot[2]
00064 
00065                 t = (goal_time - rospy.Time.now()).to_sec()
00066                 if t < .1:
00067                     t = 1.0
00068 
00069                 self.publish_command( dx / t, dy / t, dz / t )
00070 
00071                 feedback.percent_complete = t / time
00072                 self.server.publish_feedback(feedback)
00073                 r.sleep()
00074             feedback.pose_index += 1
00075 
00076         self.server.set_succeeded(MoveSequenceResult())
00077 
00078     def publish_command(self, dx, dy, dz):
00079         cmd = Twist()
00080         cmd.linear.x = dx
00081         cmd.linear.y = dy
00082         cmd.angular.z = dz
00083 
00084         if abs(cmd.linear.x) > self.tlim:
00085             cmd.linear.x = copysign(self.tlim, cmd.linear.x)
00086         if abs(cmd.linear.y) > self.tlim:
00087             cmd.linear.y = copysign(self.tlim, cmd.linear.y)
00088         if abs(cmd.angular.z) > self.rlim:
00089             cmd.angular.z = copysign(self.rlim, cmd.angular.z)
00090 
00091         self.cmd_pub.publish(cmd)
00092 
00093 if __name__=='__main__':
00094     pm = PoseMorph()
00095     rospy.spin()


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