jsk_footstep_planner: footstep_array_to_bounding_box

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from jsk_footstep_msgs.msg import Footstep, FootstepArray
00005 from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray
00006 
00007 def callback(msg):
00008     box_array = BoundingBoxArray()
00009     box_array.header = msg.header
00010     for footstep in msg.footsteps:
00011         box = BoundingBox()
00012         box.header = msg.header
00013         box.pose = footstep.pose
00014         box.dimensions = footstep.dimensions
00015         box.pose.position.z += (z_max + z_min) / 2.0
00016         box.dimensions.z = z_max - z_min
00017         box_array.boxes.append(box)
00018     pub.publish(box_array)
00019 
00020 if __name__ == "__main__":
00021     rospy.init_node("footstep_array_to_bounding_box")
00022     z_max = rospy.get_param('~z_max',0.0005)
00023     z_min = rospy.get_param('~z_min',-0.0005)
00024     pub = rospy.Publisher("~output", BoundingBoxArray)
00025     sub = rospy.Subscriber("~input", FootstepArray, callback)
00026     rospy.spin()
00027