Go to the documentation of this file.00001
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