add bbox vis

Author: KenC1014

GEMstack/onboard/perception/fusion.py

@@ -1,5 +1,6 @@
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image, PointCloud2
+from visualization_msgs.msg import MarkerArray
 from ultralytics import YOLO
 from fusion_utils import *
 import rospy
@@ -38,7 +39,7 @@ def __init__(self):
         # Publishers
         self.pub_pedestrians_pc2 = rospy.Publisher("/point_cloud/pedestrians", PointCloud2, queue_size=10)
         self.pub_centroids_pc2 = rospy.Publisher("/point_cloud/centroids", PointCloud2, queue_size=10)
-        self.pub_bbox_corners_pc2 = rospy.Publisher("/point_cloud/bbox_corners", PointCloud2, queue_size=10)
+        self.pub_bboxes_markers = rospy.Publisher("/markers/bboxes", MarkerArray, queue_size=10)
         if(self.visualization):
             self.pub_image = rospy.Publisher("/camera/image_detection", Image, queue_size=1)
 
@@ -67,7 +68,6 @@ def fusion_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2):
         # Unpacking box dimentions detected into x,y,w,h
         pedestrians_3d_centroids = []
         pedestrians_3d_dims = []
-        pedestrians_3d_bbox_corners = []
         flattened_pedestrians_2d_pts = []
         flattened_pedestrians_3d_pts = []
 
@@ -114,39 +114,36 @@ def fusion_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2):
                 dims = calculate_dimensions(extracted_3d_pts)
                 if dims != None:
                     pedestrians_3d_dims.append(dims)
-
-                # Calculate bbox corners for visualization
-                corners = calculate_bbox_corners_3d(centroid, dims)
-                if corners != None:
-                    pedestrians_3d_bbox_corners = pedestrians_3d_bbox_corners + corners
 
             # Used for visualization
             if(self.visualization):
                 cv_image = vis_2d_bbox(cv_image, xywh, box)
-        
-        if len(flattened_pedestrians_3d_pts) > 0:
-            # Draw projected 2D LiDAR points on the image.
-            for pt in flattened_pedestrians_2d_pts:
-                cv2.circle(cv_image, pt, 2, (0, 0, 255), -1)
-
-            # Create point cloud from extracted 3D points
-            ros_extracted_pedestrian_pc2 = create_point_cloud(flattened_pedestrians_3d_pts)
-            self.pub_pedestrians_pc2.publish(ros_extracted_pedestrian_pc2)
-
-        if len(pedestrians_3d_centroids) > 0:
-            # Create point cloud from pedestrain centroid
-            ros_pedestrians_centroids_pc2 = create_point_cloud(pedestrians_3d_centroids, color=(255, 0, 255))
-            self.pub_centroids_pc2.publish(ros_pedestrians_centroids_pc2)
-
-        if len(pedestrians_3d_bbox_corners) > 0:
-            # Create point cloud from pedestrain centroid
-            ros_pedestrians_bbox_corners_pc2 = create_point_cloud(pedestrians_3d_bbox_corners, color=(0, 0, 255))
-            self.pub_bbox_corners_pc2.publish(ros_pedestrians_bbox_corners_pc2)
 
         # Used for visualization
         if(self.visualization):
+            if len(flattened_pedestrians_3d_pts) > 0:
+                # Draw projected 2D LiDAR points on the image.
+                for pt in flattened_pedestrians_2d_pts:
+                    cv2.circle(cv_image, pt, 2, (0, 0, 255), -1)
+
+                # Create point cloud from extracted 3D points
+                ros_extracted_pedestrian_pc2 = create_point_cloud(flattened_pedestrians_3d_pts)
+                self.pub_pedestrians_pc2.publish(ros_extracted_pedestrian_pc2)
+
+            if len(pedestrians_3d_centroids) > 0 and len(pedestrians_3d_dims) > 0:
+                # Create point cloud from pedestrain centroid
+                ros_pedestrians_centroids_pc2 = create_point_cloud(pedestrians_3d_centroids, color=(255, 0, 255))
+                self.pub_centroids_pc2.publish(ros_pedestrians_centroids_pc2)
+
+                # Create bbox marker from pedestrain dimensions
+                ros_delete_bboxes_markers = delete_bbox_marker()
+                self.pub_bboxes_markers.publish(ros_delete_bboxes_markers)
+                ros_pedestrians_bboxes_markers = create_bbox_marker(pedestrians_3d_centroids, pedestrians_3d_dims)
+                self.pub_bboxes_markers.publish(ros_pedestrians_bboxes_markers)
+
+            # Convert cv2 image to imgmsg
             ros_img = self.bridge.cv2_to_imgmsg(cv_image, 'bgr8')
-            self.pub_image.publish(ros_img)  
+            self.pub_image.publish(ros_img)
 
 
 

GEMstack/onboard/perception/fusion_utils.py

@@ -1,4 +1,5 @@
 from sensor_msgs.msg import PointCloud2, PointField
+from visualization_msgs.msg import Marker, MarkerArray
 import numpy as np
 import sensor_msgs.point_cloud2 as pc2
 import open3d as o3d
@@ -242,4 +243,65 @@ def create_point_cloud(points, color=(255, 0, 0)):
 
     point_cloud_data = [(x, y, z, packed_color) for x, y, z in points]
 
-    return pc2.create_cloud(header, fields, point_cloud_data)
+    return pc2.create_cloud(header, fields, point_cloud_data)
+
+
+def create_bbox_marker(centroids, dimensions):
+    """
+    Create 3D bbox markers from centroids and dimensions
+    """
+    marker_array = MarkerArray()
+
+    for i, (centroid, dimension) in enumerate(zip(centroids, dimensions)):
+        # Skip if no centroid or dimension
+        if (centroid == None) or (dimension == None):
+            continue
+
+        marker = Marker()
+        marker.header.frame_id = "map"  # Reference frame
+        marker.header.stamp = rospy.Time.now()
+        marker.ns = "bounding_boxes"
+        marker.id = i  # Unique ID for each marker
+        marker.type = Marker.CUBE  # Cube for bounding box
+        marker.action = Marker.ADD
+
+        # Position (center of the bounding box)
+        marker.pose.position.x = centroid[0]
+        marker.pose.position.y = centroid[1]
+        marker.pose.position.z = centroid[2]
+
+        # Orientation (default, no rotation)
+        marker.pose.orientation.x = 0.0
+        marker.pose.orientation.y = 0.0
+        marker.pose.orientation.z = 0.0
+        marker.pose.orientation.w = 1.0
+
+        # Bounding box dimensions
+        marker.scale.x = dimension[0]
+        marker.scale.y = dimension[1]
+        marker.scale.z = dimension[2]
+
+        # Random colors for each bounding box
+        marker.color.r = 0.0  # Varying colors
+        marker.color.g = 1.0
+        marker.color.b = 1.5
+        marker.color.a = 0.2  # Transparency
+
+        marker.lifetime = rospy.Duration()  # Persistent
+        marker_array.markers.append(marker)
+    return marker_array
+
+
+def delete_bbox_marker():
+    """
+    Delete 3D bbox markers given ID ranges
+    """
+    marker_array = MarkerArray()
+    for i in range(6):
+        marker = Marker()
+        marker.ns = "bounding_boxes"
+        marker.id = i
+        marker.action = Marker.DELETE
+        marker_array.markers.append(marker)
+    return marker_array
+