Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('iri_common_smach')
00004 import rospy
00005 import smach
00006 import smach_ros
00007
00008 import tf
00009 from geometry_msgs.msg import PoseStamped
00010 from pprint import pprint
00011
00012 class GetPoseStampedFromFrames(smach.State):
00013 """
00014 Get the transformation between two frames and return it in a PoseStamped form
00015 """
00016 def __init__(self,tf_listener):
00017 smach.State.__init__(self, outcomes = ['success','fail'],
00018 input_keys = ['src_frame','dest_frame'],
00019 output_keys = ['pose_st'])
00020 self.tf_listener = tf_listener
00021
00022 def execute(self, userdata):
00023 try:
00024 self.tf_listener.waitForTransform(userdata.src_frame, userdata.dest_frame, rospy.Time(), rospy.Duration(1.0))
00025 (trans,rot) = self.tf_listener.lookupTransform(userdata.src_frame, userdata.dest_frame, rospy.Time())
00026 except (tf.LookupException, tf.ConnectivityException):
00027 rospy.logerror("Unable to get the trasform between two poses");
00028 return 'fail'
00029
00030 tf_tmp = PoseStamped()
00031 tf_tmp.header.frame_id = userdata.src_frame
00032 tf_tmp.header.stamp = rospy.Time()
00033 tf_tmp.pose.position.x = trans[0]
00034 tf_tmp.pose.position.y = trans[1]
00035 tf_tmp.pose.position.z = trans[2]
00036 tf_tmp.pose.orientation.x = rot[0]
00037 tf_tmp.pose.orientation.y = rot[1]
00038 tf_tmp.pose.orientation.z = rot[2]
00039 tf_tmp.pose.orientation.w = rot[3]
00040
00041 pprint("POSE FROM FRAMES")
00042 pprint(tf_tmp)
00043 userdata.pose_st = tf_tmp;
00044 return 'success'