diff --git a/jsk_rviz_plugins/samples/bounding_box_sample.py b/jsk_rviz_plugins/samples/bounding_box_sample.py index 6f60477f..a912b279 100755 --- a/jsk_rviz_plugins/samples/bounding_box_sample.py +++ b/jsk_rviz_plugins/samples/bounding_box_sample.py @@ -12,15 +12,19 @@ while not rospy.is_shutdown(): box_a = BoundingBox() box_b = BoundingBox() + box_c = BoundingBox() box_a.label = 2 box_b.label = 5 + box_b.label = 10 box_arr = BoundingBoxArray() now = rospy.Time.now() box_a.header.stamp = now box_b.header.stamp = now + box_c.header.stamp = now box_arr.header.stamp = now box_a.header.frame_id = "map" box_b.header.frame_id = "map" + box_c.header.frame_id = "map" box_arr.header.frame_id = "map" q = quaternion_about_axis((counter % 100) * math.pi * 2 / 100.0, [0, 0, 1]) box_a.pose.orientation.x = q[0] @@ -29,16 +33,23 @@ box_a.pose.orientation.w = q[3] box_b.pose.orientation.w = 1 box_b.pose.position.y = 2 + box_c.pose.position.y = 1 + box_c.pose.position.z = -0.52 box_b.dimensions.x = (counter % 10 + 1) * 0.1 box_b.dimensions.y = ((counter + 1) % 10 + 1) * 0.1 box_b.dimensions.z = ((counter + 2) % 10 + 1) * 0.1 box_a.dimensions.x = 1 box_a.dimensions.y = 1 box_a.dimensions.z = 1 + box_c.dimensions.x = 3 + box_c.dimensions.y = 3 + box_c.dimensions.z = 0.02 box_a.value = (counter % 100) / 100.0 box_b.value = 1 - (counter % 100) / 100.0 + box_c.value = (counter % 300) / 100.0 box_arr.boxes.append(box_a) box_arr.boxes.append(box_b) + box_arr.boxes.append(box_c) pub.publish(box_arr) r.sleep() counter = counter + 1