Added some debug code for development purposes

Author: LucasEby

GEMstack/onboard/perception/combined_detection.py

@@ -78,6 +78,7 @@ def __init__(
 
         self.yolo_topic = "/yolo_boxes"
         self.pp_topic = "/pointpillars_boxes"
+        self.debug = False
 
         rospy.loginfo(f"CombinedDetector3D Initialized. Subscribing to '{self.yolo_topic}' and '{self.pp_topic}'.")
 
@@ -93,6 +94,7 @@ def state_outputs(self) -> list:
     def initialize(self):
         self.yolo_sub = Subscriber(self.yolo_topic, BoundingBoxArray)
         self.pp_sub = Subscriber(self.pp_topic, BoundingBoxArray)
+        self.pub_fused = rospy.Publisher("/fused_boxes", BoundingBoxArray, queue_size=1)
 
         queue_size = 10
         slop = 0.1
@@ -140,6 +142,7 @@ def _fuse_bounding_boxes(self,
                              vehicle_state: VehicleState,
                              current_time: float
                             ) -> Dict[str, AgentState]:
+        original_header = yolo_bbx_array.header
         current_agents_in_frame: Dict[str, AgentState] = {}
         yolo_boxes: List[BoundingBox] = yolo_bbx_array.boxes
         pp_boxes: List[BoundingBox] = pp_bbx_array.boxes
@@ -183,7 +186,13 @@ def _fuse_bounding_boxes(self,
                 fused_boxes_list.append(pp_box)
                 rospy.logdebug(f"Kept unmatched PP box {j}")
 
-        # Agenstate
+        if self.debug:
+            # Work in progress to visualize combined results
+            fused_array = BoundingBoxArray()
+            fused_array.header = yolo_bbx_array.header
+            fused_array.boxes = fused_boxes_list
+            self.pub_fused.publish(fused_array)
+
         for i, box in enumerate(fused_boxes_list):
             try:
                  # Cur vehicle frame

GEMstack/onboard/perception/yolo_node.py

@@ -253,6 +253,7 @@ def __init__(
         self.camera_name = 'front'
         self.camera_front = (self.camera_name=='front')
         self.score_threshold = 0.4
+        self.debug = True
         self.initialize()
 
     def initialize(self):
@@ -463,6 +464,20 @@ def synchronized_callback(self, image_msg, lidar_msg):
             box_msg.dimensions.x = float(dims[0])  # length
             box_msg.dimensions.y = float(dims[1])  # width
             box_msg.dimensions.z = float(dims[2])  # height
+
+            if self.debug:
+                print("X")
+                print(center_vehicle[0])
+                print("L")
+                print(dims[0])
+                print("Y")
+                print(center_vehicle[1])
+                print("W")
+                print(dims[1])
+                print("Z")
+                print(center_vehicle[2])
+                print("H")
+                print(dims[2])
 
             # Add confidence score and label
             box_msg.value = float(conf_scores[i])