footstep_array_to_bounding_box_array.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 from jsk_footstep_msgs.msg import Footstep, FootstepArray
5 from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray
6 
7 def callback(msg):
8  box_array = BoundingBoxArray()
9  box_array.header = msg.header
10  for footstep in msg.footsteps:
11  box = BoundingBox()
12  box.header = msg.header
13  box.pose = footstep.pose
14  box.dimensions = footstep.dimensions
15  box.pose.position.z += (z_max + z_min) / 2.0
16  box.dimensions.z = z_max - z_min
17  box_array.boxes.append(box)
18  pub.publish(box_array)
19 
20 if __name__ == "__main__":
21  rospy.init_node("footstep_array_to_bounding_box")
22  z_max = rospy.get_param('~z_max',0.0005)
23  z_min = rospy.get_param('~z_min',-0.0005)
24  pub = rospy.Publisher("~output", BoundingBoxArray)
25  sub = rospy.Subscriber("~input", FootstepArray, callback)
26  rospy.spin()
27 


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri Jul 26 2019 03:54:32