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
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()