create launch files and fix post synchronizer for combined detections

src/perception/camera_object_detection/camera_object_detection/post_synchronizer.py

@@ -3,33 +3,53 @@
 from message_filters import Subscriber, ApproximateTimeSynchronizer
 
 from sensor_msgs.msg import Image
-from vision_msgs.msg import Detection2DArray, Detection2D
+from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose
 from std_msgs.msg import Header
 
 from ultralytics.utils.plotting import Annotator, colors
 
-from cv_bridge import CvBridgeError
+from cv_bridge import CvBridge, CvBridgeError
 
 class CameraSyncNode(Node): # synchronizes visualizations
     def __init__(self):
         super().__init__('camera_sync_node')
         self.get_logger().info("Camera Sync Node")
 
-        self.camera_img_sub = Subscriber(self, Image , '/camera/right/image_color')
-
-        self.camera1_sub = Subscriber(self, Detection2DArray, '/camera/right/camera_detections')
-        self.camera2_sub = Subscriber(self, Detection2DArray, '/traffic_signs')
-        self.camera3_sub = Subscriber(self, Detection2DArray, '/traffic_lights')
+        self.declare_parameter("camera_img_topic", "/camera/right/image_color")
+        self.declare_parameter("camera_detection_topic", "/camera/right/camera_detections")
+        self.declare_parameter("traffic_signs_topic", "/traffic_signs/right")
+        self.declare_parameter("traffic_lights_topic", "/traffic_lights/right")
+        self.declare_parameter("combined_detection_publisher", "/camera/right/combined_detections")
+        self.declare_parameter("publish_vis_topic", "/camera/right/annotated_img")
+
+        self.camera_img_topic = self.get_parameter("camera_img_topic").value
+        self.camera_detection_topic = self.get_parameter("camera_detection_topic").value
+        self.traffic_signs_topic = self.get_parameter("traffic_signs_topic").value
+        self.traffic_lights_topic = self.get_parameter("traffic_lights_topic").value
+        self.combined_detection_topic = self.get_parameter("combined_detection_publisher").value
+        self.publish_vis_topic = self.get_parameter("publish_vis_topic").value
+
+        self.camera_img_sub = Subscriber(self, Image, self.camera_img_topic)
+        self.camera_detection_sub = Subscriber(self, Detection2DArray, self.camera_detection_topic)
+        self.traffic_signs_sub = Subscriber(self, Detection2DArray, self.traffic_signs_topic)
+        self.traffic_lights_sub = Subscriber(self, Detection2DArray, self.traffic_lights_topic)
+
+        self.cv_bridge = CvBridge()
+        self.line_thickness = 1
+        self.names = []
 
         self.ts = ApproximateTimeSynchronizer(
-            [self.camera_img_sub, self.camera1_sub, self.camera2_sub, self.camera3_sub],
+            [self.camera_img_sub,
+             self.camera_detection_sub,
+             self.traffic_signs_sub,
+             self.traffic_lights_sub],
             queue_size=10,
             slop=0.1)
 
         self.ts.registerCallback(self.callback)
 
-        self.combined_detection_publisher = self.create_publisher(Detection2DArray, '/combined_detections', 10)
-        self.vis_publisher = self.create_publisher(Image, '/annotated_img', 10)
+        self.combined_detection_publisher = self.create_publisher(Detection2DArray, self.combined_detection_topic, 10)
+        self.vis_publisher = self.create_publisher(Image, self.publish_vis_topic, 10)
 
     def process_img(self, image):
         try:
@@ -67,38 +87,76 @@ def postprocess_detections(self, detections, annotator):
         annotator_img = annotator.result()
         return (processed_detections, annotator_img)
 
+    def publish_detections(self, detections, msg, feed):
+        # Publish detections to an detectionList message
+        detection2darray = Detection2DArray()
+
+        # fill header for detection list
+        detection2darray.header.stamp = msg.header.stamp
+        detection2darray.header.frame_id = msg.header.frame_id
+        # populate detection list
+        if detections is not None and len(detections):
+            for detection in detections:
+                detection2d = Detection2D()
+                detection2d.header.stamp = self.get_clock().now().to_msg()
+                detection2d.header.frame_id = msg.header.frame_id
+                detected_object = ObjectHypothesisWithPose()
+                detected_object.hypothesis.class_id = detection["label"]
+                detected_object.hypothesis.score = detection["conf"]
+                detection2d.results.append(detected_object)
+                detection2d.bbox.center.position.x = detection["bbox"][0]
+                detection2d.bbox.center.position.y = detection["bbox"][1]
+                detection2d.bbox.size_x = detection["bbox"][2]
+                detection2d.bbox.size_y = detection["bbox"][3]
+
+                # append detection to detection list
+                detection2darray.detections.append(detection2d)
+
+        self.combined_detection_publisher.publish(detection2darray)
+
     def publish_vis(self, annotated_img, msg):
         # Publish visualizations
         imgmsg = self.cv_bridge.cv2_to_imgmsg(annotated_img, "bgr8")
         imgmsg.header.stamp = msg.header.stamp
         imgmsg.header.frame_id = msg.header.frame_id
+        self.get_logger().info("Publish img")
         self.vis_publisher.publish(imgmsg)
 
-    def callback(self, camera_img_sub, camera1_msg, camera2_msg, camera3_msg):
-        combined_detections = Detection2DArray()
-        combined_detections.header = Header()
-        combined_detections.header.stamp = self.get_clock().now().to_msg()
-        combined_detections.header.frame_id = camera1_msg.header.frame_id
-
+    def callback(
+        self,
+        camera_img_sub,
+        camera_detection_sub,
+        traffic_signs_sub,
+        traffic_lights_sub
+    ):
         cv_image = self.process_img(camera_img_sub)
-
-        for detection in camera1_msg.detections:
-            combined_detections.detections.append(detection)
-
-        for detection in camera2_msg.detections:
-            combined_detections.detections.append(detection)
-
-        for detection in camera3_msg.detections:
-            combined_detections.detections.append(detection)
+
+        combined_detections = camera_detection_sub.detections + traffic_lights_sub.detections + traffic_signs_sub.detections
 
         annotator = Annotator(
             cv_image,
             line_width=self.line_thickness,
             example=str(self.names),
         )
-        (combined_detections, annotated_img) = self.postprocess_detections(combined_detections, annotator)
-
-        self.combined_detection_publisher.publish(combined_detections)
+
+        detections = []
+        for det in combined_detections:
+            obj_with_pose = det.results[0]
+            detections.append(
+                {
+                    "label": obj_with_pose.hypothesis.class_id,
+                    "conf": obj_with_pose.hypothesis.score,
+                    "bbox": [
+                        det.bbox.center.position.x,
+                        det.bbox.center.position.y,
+                        det.bbox.size_x,
+                        det.bbox.size_y
+                    ],
+                }
+            )
+        (combined_detections, annotated_img) = self.postprocess_detections(detections, annotator)
+
+        self.publish_detections(combined_detections, camera_detection_sub, "")
         self.publish_vis(annotated_img, camera_img_sub)
 
 def main(args=None):

src/perception/camera_object_detection/config/post_synchronizer_config.yaml

@@ -1,4 +1,26 @@
-post_synchronizer_node:
+left_post_synchronizer_node:
   ros__parameters:
-    camera_topic: /camera/right/image_color
-    publish_vis_topic: /annotated_img
+    camera_img_topic: /camera/left/image_color
+    camera_detection_topic: /camera/left/camera_detections
+    traffic_signs_topic: /traffic_signs/left
+    traffic_lights_topic: /traffic_lights/left
+    combined_detection_publisher: /camera/left/combined_detections
+    publish_vis_topic: /camera/left/annotated_img
+
+center_post_synchronizer_node:
+  ros__parameters:
+    camera_img_topic: /camera/center/image_color
+    camera_detection_topic: /camera/center/camera_detections
+    traffic_signs_topic: /traffic_signs/center
+    traffic_lights_topic: /traffic_lights/center
+    combined_detection_publisher: /camera/center/combined_detections
+    publish_vis_topic: /camera/center/annotated_img
+
+right_post_synchronizer_node:
+  ros__parameters:
+    camera_img_topic: /camera/right/image_color
+    camera_detection_topic: /camera/right/camera_detections
+    traffic_signs_topic: /traffic_signs/right
+    traffic_lights_topic: /traffic_lights/right
+    combined_detection_publisher: /camera/right/combined_detections
+    publish_vis_topic: /camera/right/annotated_img

src/perception/camera_object_detection/config/traffic_light_config.yaml

@@ -1,9 +1,31 @@
-traffic_light_node:
+left_traffic_light_node:
   ros__parameters:
-    camera_topic: /camera/left/image_color
+      camera_topic: /camera/left/image_color
+      publish_vis_topic: /traffic_lights_viz
+      publish_detection_topic: /traffic_lights/left
+      model_path: /perception_models/traffic_light.pt
+      crop_mode: CenterCrop
+      image_size: 1024
+      save_detections: false
+
+center_traffic_light_node:
+  ros__parameters:
+      camera_topic: /camera/center/image_color
+      publish_vis_topic: /traffic_lights_viz
+      publish_detection_topic: /traffic_lights/center
+      model_path: /perception_models/traffic_light.pt
+      crop_mode: CenterCrop
+      image_size: 1024
+      save_detections: false
+
+right_traffic_light_node:
+  ros__parameters:
+    camera_topic: /camera/right/image_color
     publish_vis_topic: /traffic_lights_viz
-    publish_detection_topic: /traffic_lights
+    publish_detection_topic: /traffic_lights/right
     model_path: /perception_models/traffic_light.pt
     crop_mode: CenterCrop
     image_size: 1024
     save_detections: false
+
+

src/perception/camera_object_detection/config/traffic_signs_config.yaml

@@ -1,8 +1,28 @@
-traffic_signs_node:
+right_traffic_signs_node:
   ros__parameters:
     camera_topic: /camera/left/image_color
     publish_vis_topic: /traffic_signs_viz
-    publish_detection_topic: /traffic_signs
+    publish_detection_topic: /traffic_signs/left
+    model_path: /perception_models/traffic_signs.pt
+    crop_mode: CenterCrop
+    image_size: 1024
+    save_detections: false
+
+center_traffic_signs_node:
+  ros__parameters:
+    camera_topic: /camera/center/image_color
+    publish_vis_topic: /traffic_signs_viz
+    publish_detection_topic: /traffic_signs/center
+    model_path: /perception_models/traffic_signs.pt
+    crop_mode: CenterCrop
+    image_size: 1024
+    save_detections: false
+
+right_traffic_signs_node:
+  ros__parameters:
+    camera_topic: /camera/right/image_color
+    publish_vis_topic: /traffic_signs_viz
+    publish_detection_topic: /traffic_signs/right
     model_path: /perception_models/traffic_signs.pt
     crop_mode: CenterCrop
     image_size: 1024

src/perception/camera_object_detection/launch/include/post_synchronizer.launch.py

@@ -11,11 +11,31 @@ def generate_launch_description():
         'post_synchronizer_config.yaml'
     )
 
-    camera_object_detection_node = Node(
+    left_camera_object_detection_node = Node(
         package='camera_object_detection',
         executable='camera_sync_node',
-        name='post_synchronizer_node',
+        name='left_post_synchronizer_node',
         parameters=[config]
     )
 
-    return LaunchDescription([camera_object_detection_node])
+    center_camera_object_detection_node = Node(
+        package='camera_object_detection',
+        executable='camera_sync_node',
+        name='center_post_synchronizer_node',
+        parameters=[config]
+    )
+
+    right_camera_object_detection_node = Node(
+        package='camera_object_detection',
+        executable='camera_sync_node',
+        name='right_post_synchronizer_node',
+        parameters=[config]
+    )
+
+    return LaunchDescription(
+        [
+            left_camera_object_detection_node,
+            center_camera_object_detection_node,
+            right_camera_object_detection_node
+        ]
+    )

src/perception/camera_object_detection/launch/include/traffic_light.launch.py

@@ -11,11 +11,31 @@ def generate_launch_description():
         'traffic_light_config.yaml'
     )
 
-    camera_object_detection_node = Node(
+    left_camera_object_detection_node = Node(
         package='camera_object_detection',
         executable='camera_object_detection_node',
-        name='traffic_light_node',
+        name='left_traffic_light_node',
         parameters=[config]
     )
 
-    return LaunchDescription([camera_object_detection_node])
+    center_camera_object_detection_node = Node(
+        package='camera_object_detection',
+        executable='camera_object_detection_node',
+        name='center_traffic_light_node',
+        parameters=[config]
+    )
+
+    right_camera_object_detection_node = Node(
+        package='camera_object_detection',
+        executable='camera_object_detection_node',
+        name='right_traffic_light_node',
+        parameters=[config]
+    )
+
+    return LaunchDescription(
+        [
+            left_camera_object_detection_node,
+            center_camera_object_detection_node,
+            right_camera_object_detection_node
+        ]
+    )

src/perception/camera_object_detection/launch/include/traffic_signs.launch.py

@@ -11,11 +11,31 @@ def generate_launch_description():
         'traffic_signs_config.yaml'
     )
 
-    camera_object_detection_node = Node(
+    left_camera_object_detection_node = Node(
         package='camera_object_detection',
         executable='camera_object_detection_node',
-        name='traffic_signs_node',
+        name='left_traffic_signs_node',
         parameters=[config]
     )
 
-    return LaunchDescription([camera_object_detection_node])
+    center_camera_object_detection_node = Node(
+        package='camera_object_detection',
+        executable='camera_object_detection_node',
+        name='center_traffic_signs_node',
+        parameters=[config]
+    )
+
+    right_camera_object_detection_node = Node(
+        package='camera_object_detection',
+        executable='camera_object_detection_node',
+        name='right_traffic_signs_node',
+        parameters=[config]
+    )
+
+    return LaunchDescription(
+        [
+            left_camera_object_detection_node,
+            center_camera_object_detection_node,
+            right_camera_object_detection_node
+        ]
+    )