Cleaned up bounding box creation process for YOLO node and moved to another file. Added rest of publishing code to combined_detection.py

Author: LucasEby

GEMstack/onboard/perception/combined_detection.py

@@ -195,6 +195,8 @@ def _fuse_bounding_boxes(self,
 
         for i, box in enumerate(fused_boxes_list):
             try:
+                fuxed_bb_array.boxes.append(box)
+
                  # Cur vehicle frame
                 pos_x = box.pose.position.x; pos_y = box.pose.position.y; pos_z = box.pose.position.z
                 quat_x = box.pose.orientation.x; quat_y = box.pose.orientation.y; quat_z = box.pose.orientation.z; quat_w = box.pose.orientation.w

GEMstack/onboard/perception/combined_detection_utils.py

@@ -0,0 +1,36 @@
+from jsk_recognition_msgs.msg import BoundingBox
+from scipy.spatial.transform import Rotation as R
+
+# Seperated Sensor Fusion specific utilities into it's own file because package imports are required 
+# (to minimize integration problems)
+
+# Adds a new bounding box to a BoundingBoxArray
+def add_bounding_box(boxes, frame_id, stamp, x, y, z, l, w, h, yaw, conf_score, label):
+    box_msg = BoundingBox()
+    box_msg.header.frame_id = frame_id
+    box_msg.header.stamp = stamp
+    
+    # Set the pose
+    box_msg.pose.position.x = float(x)
+    box_msg.pose.position.y = float(y)
+    box_msg.pose.position.z = float(z)
+    
+    # Convert yaw to quaternion
+    quat = R.from_euler('z', yaw).as_quat()
+    box_msg.pose.orientation.x = float(quat[0])
+    box_msg.pose.orientation.y = float(quat[1])
+    box_msg.pose.orientation.z = float(quat[2])
+    box_msg.pose.orientation.w = float(quat[3])
+    
+    # Set the dimensions
+    # Swapped dims[2] and dims[0]
+    box_msg.dimensions.x = float(l)  # length
+    box_msg.dimensions.y = float(w)  # width
+    box_msg.dimensions.z = float(h)  # height
+
+    # Add confidence score and label
+    box_msg.value = float(conf_score)
+    box_msg.label = label
+    
+    boxes.boxes.append(box_msg)
+    return boxes

GEMstack/onboard/perception/yolo_node.py

@@ -1,4 +1,5 @@
 from pedestrian_utils import *
+from combined_detection_utils import add_bounding_box
 from ultralytics import YOLO
 import cv2
 from scipy.spatial.transform import Rotation as R
@@ -9,8 +10,8 @@
 import time
 
 # Publisher imports:
-from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray
-from geometry_msgs.msg import Pose, Vector3
+from jsk_recognition_msgs.msg import BoundingBoxArray
+# from geometry_msgs.msg import Pose, Vector3
 
 
 class YoloNode():
@@ -220,49 +221,20 @@ def synchronized_callback(self, image_msg, lidar_msg):
             yaw = np.arctan2(R_vehicle[1, 0], R_vehicle[0, 0])
 
             refined_center = refined_center_vehicle
-            
-            # Create a ROS BoundingBox message
-            box_msg = BoundingBox()
-            box_msg.header.frame_id = 'currentVehicleFrame'
-            box_msg.header.stamp = lidar_msg.header.stamp
-            
-            # Set the pose
-            box_msg.pose.position.x = float(refined_center_vehicle[0])
-            box_msg.pose.position.y = float(refined_center_vehicle[1])
-            box_msg.pose.position.z = float(refined_center_vehicle[2])
-            
-            # Convert yaw to quaternion
-            quat = R.from_euler('z', yaw).as_quat()
-            box_msg.pose.orientation.x = float(quat[0])
-            box_msg.pose.orientation.y = float(quat[1])
-            box_msg.pose.orientation.z = float(quat[2])
-            box_msg.pose.orientation.w = float(quat[3])
-            
-            # Set the dimensions
-            # Swapped dims[2] and dims[0]
-            box_msg.dimensions.x = float(dims[2])  # length
-            box_msg.dimensions.y = float(dims[1])  # width
-            box_msg.dimensions.z = float(dims[0])  # height
 
-            # if self.debug:
-            #     print("X")
-            #     print(refined_center_vehicle[0])
-            #     print("L")
-            #     print(dims[0])
-            #     print("Y")
-            #     print(refined_center_vehicle[1])
-            #     print("W")
-            #     print(dims[1])
-            #     print("Z")
-            #     print(refined_center_vehicle[2])
-            #     print("H")
-            #     print(dims[2])
-            
-            # Add confidence score and label
-            box_msg.value = float(conf_scores[i])
-            box_msg.label = 0  # person/pedestrian class
-            
-            boxes.boxes.append(box_msg)
+            boxes = add_bounding_box(boxes=boxes, 
+                frame_id='currentVehicleFrame', 
+                stamp=lidar_msg.header.stamp, 
+                x=refined_center_vehicle[0], 
+                y=refined_center_vehicle[1], 
+                z=refined_center_vehicle[2], 
+                l=dims[2], # length 
+                w=dims[1], # width 
+                h=dims[0], # height 
+                yaw=yaw,
+                conf_score=float(conf_scores[i]),
+                label=0 # person/pedestrian class
+            )
 
             rospy.loginfo(f"Person detected at ({refined_center_vehicle[0]:.2f}, {refined_center_vehicle[1]:.2f}, {refined_center_vehicle[2]:.2f}) with score {conf_scores[i]:.2f}")