transform_footstep_array.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 import sys
5 from jsk_footstep_msgs.msg import Footstep, FootstepArray
6 import tf
7 from tf.transformations import *
8 
9 def callback(msg):
10  global tf_listener, frame_id, pub
11  try:
12  if strict_tf:
13  tf_listener.waitForTransform(frame_id, msg.header.frame_id,
14  msg.header.stamp,
15  rospy.Duration(1.0))
16  if strict_tf:
17  stamp = msg.header.stamp
18  else:
19  stamp = rospy.Time(0.0)
20  # frame_id -> header
21  (pos, rot) = tf_listener.lookupTransform(frame_id, msg.header.frame_id,
22  stamp)
23  trans = concatenate_matrices(translation_matrix(pos), quaternion_matrix(rot))
24  new_msg = FootstepArray()
25  new_msg.header.frame_id = frame_id
26  new_msg.header.stamp = msg.header.stamp
27  for footstep in msg.footsteps:
28  new_footstep = Footstep()
29  new_footstep.leg = footstep.leg
30  new_footstep.duration = footstep.duration
31  new_footstep.footstep_group = footstep.footstep_group
32  new_footstep.swing_height = footstep.swing_height
33  new_footstep.dimensions = footstep.dimensions
34  old_pose = concatenate_matrices(translation_matrix([
35  footstep.pose.position.x,
36  footstep.pose.position.y,
37  footstep.pose.position.z]),
38  quaternion_matrix([
39  footstep.pose.orientation.x,
40  footstep.pose.orientation.y,
41  footstep.pose.orientation.z,
42  footstep.pose.orientation.w]),
43  )
44  new_pose = concatenate_matrices(trans, old_pose)
45  translation = translation_from_matrix(new_pose)
46  rotation = quaternion_from_matrix(new_pose)
47  new_footstep.pose.position.x = translation[0]
48  new_footstep.pose.position.y = translation[1]
49  new_footstep.pose.position.z = translation[2]
50  new_footstep.pose.orientation.x = rotation[0]
51  new_footstep.pose.orientation.y = rotation[1]
52  new_footstep.pose.orientation.z = rotation[2]
53  new_footstep.pose.orientation.w = rotation[3]
54  new_msg.footsteps.append(new_footstep)
55  pub.publish(new_msg)
56  except tf.Exception as e:
57  rospy.logerr("[transform_footstep_array] Failed to lookup transform: %s" % (e.message))
58 
59 
60 if __name__ == "__main__":
61  rospy.init_node("transform_footstep_array")
62  tf_listener = tf.TransformListener() # Should we use tf2?
63  pub = rospy.Publisher("~output", FootstepArray)
64  frame_id = rospy.get_param("~frame_id", "odom")
65  strict_tf = rospy.get_param("~strict_tf", True)
66  s = rospy.Subscriber("~input", FootstepArray, callback)
67  rospy.spin()
68 


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Sun May 28 2023 03:03:19