jsk_footstep_planner: transform_footstep

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import sys
00005 from jsk_footstep_msgs.msg import Footstep, FootstepArray
00006 import tf
00007 from tf.transformations import *
00008 
00009 def callback(msg):
00010     global tf_listener, frame_id, pub
00011     try:
00012         if strict_tf:
00013             tf_listener.waitForTransform(frame_id, msg.header.frame_id,
00014                                          msg.header.stamp,
00015                                          rospy.Duration(1.0))
00016         if strict_tf:
00017             stamp = msg.header.stamp
00018         else:
00019             stamp = rospy.Time(0.0)
00020         # frame_id -> header
00021         (pos, rot) = tf_listener.lookupTransform(frame_id, msg.header.frame_id,
00022                                                  stamp)
00023         trans = concatenate_matrices(translation_matrix(pos), quaternion_matrix(rot))
00024         new_msg = FootstepArray()
00025         new_msg.header.frame_id = frame_id
00026         new_msg.header.stamp = msg.header.stamp
00027         for footstep in msg.footsteps:
00028             new_footstep = Footstep()
00029             new_footstep.leg = footstep.leg
00030             new_footstep.duration = footstep.duration
00031             new_footstep.footstep_group = footstep.footstep_group
00032             new_footstep.swing_height = footstep.swing_height
00033             new_footstep.dimensions = footstep.dimensions
00034             old_pose = concatenate_matrices(translation_matrix([
00035                 footstep.pose.position.x,
00036                 footstep.pose.position.y,
00037                 footstep.pose.position.z]),
00038                                             quaternion_matrix([
00039                                                 footstep.pose.orientation.x,
00040                                                 footstep.pose.orientation.y,
00041                                                 footstep.pose.orientation.z,
00042                                                 footstep.pose.orientation.w]),
00043             )
00044             new_pose = concatenate_matrices(trans, old_pose)
00045             translation = translation_from_matrix(new_pose)
00046             rotation = quaternion_from_matrix(new_pose)
00047             new_footstep.pose.position.x = translation[0]
00048             new_footstep.pose.position.y = translation[1]
00049             new_footstep.pose.position.z = translation[2]
00050             new_footstep.pose.orientation.x = rotation[0]
00051             new_footstep.pose.orientation.y = rotation[1]
00052             new_footstep.pose.orientation.z = rotation[2]
00053             new_footstep.pose.orientation.w = rotation[3]
00054             new_msg.footsteps.append(new_footstep)
00055         pub.publish(new_msg)
00056     except tf.Exception, e:
00057         rospy.logerr("[transform_footstep_array] Failed to lookup transform: %s" % (e.message))
00058     
00059 
00060 if __name__ == "__main__":
00061     rospy.init_node("transform_footstep_array")
00062     tf_listener = tf.TransformListener() # Should we use tf2?
00063     pub = rospy.Publisher("~output", FootstepArray)
00064     frame_id = rospy.get_param("~frame_id", "odom")
00065     strict_tf = rospy.get_param("~strict_tf", True)
00066     s = rospy.Subscriber("~input", FootstepArray, callback)
00067     rospy.spin()
00068