diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml index d30474f9..b81eac61 100644 --- a/modules/docker-compose.perception.yaml +++ b/modules/docker-compose.perception.yaml @@ -29,11 +29,11 @@ services: - driver: nvidia count: 1 capabilities: [ gpu ] - command: /bin/bash -c "ros2 launch camera_object_detection eve_launch.py" + command: /bin/bash -c "ros2 launch camera_object_detection eve.launch.py" volumes: - /mnt/wato-drive2/perception_models/yolov8m.pt:/perception_models/yolov8m.pt - /mnt/wato-drive2/perception_models/traffic_light.pt:/perception_models/traffic_light.pt - - /mnt/wato-drive2/perception_models/traffic_signs_v0.pt:/perception_models/traffic_signs_v1.pt + - /mnt/wato-drive2/perception_models/traffic_signs_v3.pt:/perception_models/traffic_signs.pt lidar_object_detection: build: diff --git a/src/perception/camera_object_detection/camera_object_detection/post_synchronizer.py b/src/perception/camera_object_detection/camera_object_detection/post_synchronizer.py new file mode 100644 index 00000000..d591c6d8 --- /dev/null +++ b/src/perception/camera_object_detection/camera_object_detection/post_synchronizer.py @@ -0,0 +1,172 @@ +import rclpy +from rclpy.node import Node +from message_filters import Subscriber, ApproximateTimeSynchronizer + +from sensor_msgs.msg import Image +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 CvBridge, CvBridgeError + +class CameraSyncNode(Node): # synchronizes visualizations + def __init__(self): + super().__init__('camera_sync_node') + self.get_logger().info("Camera Sync Node") + + 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.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, self.combined_detection_topic, 10) + self.vis_publisher = self.create_publisher(Image, self.publish_vis_topic, 10) + + def process_img(self, image): + try: + cv_image = self.cv_bridge.imgmsg_to_cv2(image, desired_encoding="passthrough") + except CvBridgeError as e: + self.get_logger().error(str(e)) + return + return cv_image + + def postprocess_detections(self, detections, annotator): + """ + Post-process draws bouningboxes on camera image. + + Parameters: + detections: A list of dict with the format + { + "label": str, + "bbox": [float], + "conf": float + } + annotator: A ultralytics.yolo.utils.plotting.Annotator for the current image + + Returns: + processed_detections: filtered detections + annotator_img: image with bounding boxes drawn on + """ + processed_detections = detections + + for det in detections: + label = f'{det["label"]} {det["conf"]:.2f}' + x1, y1, w1, h1 = det["bbox"] + xyxy = [x1, y1, x1 + w1, y1 + h1] + annotator.box_label(xyxy, label, color=colors(1, True)) + + 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, + camera_detection_sub, + traffic_signs_sub, + traffic_lights_sub + ): + cv_image = self.process_img(camera_img_sub) + + 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), + ) + + 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): + rclpy.init(args=args) + node = CameraSyncNode() + + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/perception/camera_object_detection/camera_object_detection/yolov8_detection.py b/src/perception/camera_object_detection/camera_object_detection/yolov8_detection.py index 05ceb431..c01e408e 100755 --- a/src/perception/camera_object_detection/camera_object_detection/yolov8_detection.py +++ b/src/perception/camera_object_detection/camera_object_detection/yolov8_detection.py @@ -281,16 +281,16 @@ def image_callback(self, msg): ) self.get_logger().debug(f"{label}: {bbox}") - annotator = Annotator( - cv_image, - line_width=self.line_thickness, - example=str(self.names), - ) - (detections, annotated_img) = self.postprocess_detections(detections, annotator) + # annotator = Annotator( + # cv_image, + # line_width=self.line_thickness, + # example=str(self.names), + # ) + # (detections, annotated_img) = self.postprocess_detections(detections, annotator) # Currently we support a single camera so we pass an empty string feed = "" - self.publish_vis(annotated_img, msg, feed) + # self.publish_vis(annotated_img, msg, feed) self.publish_detections(detections, msg, feed) if self.save_detections: diff --git a/src/perception/camera_object_detection/config/post_synchronizer_config.yaml b/src/perception/camera_object_detection/config/post_synchronizer_config.yaml new file mode 100644 index 00000000..9bee55fd --- /dev/null +++ b/src/perception/camera_object_detection/config/post_synchronizer_config.yaml @@ -0,0 +1,26 @@ +left_post_synchronizer_node: + ros__parameters: + 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 diff --git a/src/perception/camera_object_detection/config/traffic_light_config.yaml b/src/perception/camera_object_detection/config/traffic_light_config.yaml index 9a3e00c8..6306cff3 100755 --- a/src/perception/camera_object_detection/config/traffic_light_config.yaml +++ b/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 + + diff --git a/src/perception/camera_object_detection/config/traffic_signs_config.yaml b/src/perception/camera_object_detection/config/traffic_signs_config.yaml index 8a143a77..f0ba0a80 100644 --- a/src/perception/camera_object_detection/config/traffic_signs_config.yaml +++ b/src/perception/camera_object_detection/config/traffic_signs_config.yaml @@ -1,9 +1,29 @@ -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 - model_path: /perception_models/traffic_signs_v1.pt + 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 save_detections: false diff --git a/src/perception/camera_object_detection/launch/eve.launch.py b/src/perception/camera_object_detection/launch/eve.launch.py index 9ff4992e..19af41a0 100755 --- a/src/perception/camera_object_detection/launch/eve.launch.py +++ b/src/perception/camera_object_detection/launch/eve.launch.py @@ -47,11 +47,18 @@ def generate_launch_description(): condition=LaunchConfigurationEquals("launch_traffic_signs", "True"), ) + post_synchronizer_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [camera_object_detection_launch_include_dir, "/post_synchronizer.launch.py"] + ) + ) + return LaunchDescription( launch_args + [ pretrained_yolov8_launch, traffic_light_launch, traffic_signs_launch, + post_synchronizer_launch, ] ) diff --git a/src/perception/camera_object_detection/launch/include/post_synchronizer.launch.py b/src/perception/camera_object_detection/launch/include/post_synchronizer.launch.py new file mode 100644 index 00000000..3da3aee5 --- /dev/null +++ b/src/perception/camera_object_detection/launch/include/post_synchronizer.launch.py @@ -0,0 +1,41 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('camera_object_detection'), + 'config', + 'post_synchronizer_config.yaml' + ) + + left_camera_object_detection_node = Node( + package='camera_object_detection', + executable='camera_sync_node', + name='left_post_synchronizer_node', + parameters=[config] + ) + + 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 + ] + ) diff --git a/src/perception/camera_object_detection/launch/include/traffic_light.launch.py b/src/perception/camera_object_detection/launch/include/traffic_light.launch.py index 234844b8..fcf7454a 100755 --- a/src/perception/camera_object_detection/launch/include/traffic_light.launch.py +++ b/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 + ] + ) diff --git a/src/perception/camera_object_detection/launch/include/traffic_signs.launch.py b/src/perception/camera_object_detection/launch/include/traffic_signs.launch.py index cdb0134f..a8c0c05c 100644 --- a/src/perception/camera_object_detection/launch/include/traffic_signs.launch.py +++ b/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 + ] + ) diff --git a/src/perception/camera_object_detection/setup.py b/src/perception/camera_object_detection/setup.py index c9665f79..cddea271 100755 --- a/src/perception/camera_object_detection/setup.py +++ b/src/perception/camera_object_detection/setup.py @@ -25,7 +25,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'camera_object_detection_node = camera_object_detection.yolov8_detection:main' + 'camera_object_detection_node = camera_object_detection.yolov8_detection:main', + 'camera_sync_node = camera_object_detection.post_synchronizer:main' ], }, )