add 3d bbox ref frame

Author: KenC1014

GEMstack/onboard/perception/cone_detection_parking.py

@@ -354,7 +354,7 @@ def viz_object_states(self, cv_image, boxes):
             # Create bbox markers from cone dimensions
             ros_delete_bboxes_markers = delete_bbox_marker()
             self.pub_cones_bboxes_markers.publish(ros_delete_bboxes_markers)
-            ros_cones_bboxes_markers = create_bbox_marker(cone_3d_centers, cone_3d_dims)
+            ros_cones_bboxes_markers = create_bbox_marker(cone_3d_centers, cone_3d_dims, "lidar_top")
             self.pub_cones_bboxes_markers.publish(ros_cones_bboxes_markers)
 
 

GEMstack/onboard/perception/visualization_utils.py

@@ -64,7 +64,7 @@ def create_point_cloud(points, color=(255, 0, 0)):
 
     return pc2.create_cloud(header, fields, point_cloud_data)
 
-def create_bbox_marker(centroids, dimensions):
+def create_bbox_marker(centroids, dimensions, ref_frame="map"):
     """
     Create 3D bbox markers from centroids and dimensions
     """
@@ -76,7 +76,7 @@ def create_bbox_marker(centroids, dimensions):
             continue
 
         marker = Marker()
-        marker.header.frame_id = "top_lidar"  # Reference frame
+        marker.header.frame_id = ref_frame  # Reference frame
         marker.header.stamp = rospy.Time.now()
         marker.ns = "bounding_boxes"
         marker.id = i  # Unique ID for each marker