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 index 46d5ac62..88537e0b 100644 --- a/src/perception/camera_object_detection/camera_object_detection/post_synchronizer.py +++ b/src/perception/camera_object_detection/camera_object_detection/post_synchronizer.py @@ -13,12 +13,13 @@ 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/left/camera_detections') - self.camera2_sub = Subscriber(self, Detection2DArray, '/camera/center/camera_detections') - self.camera3_sub = Subscriber(self, Detection2DArray, '/camera/right/camera_detections') + 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.ts = ApproximateTimeSynchronizer( [self.camera_img_sub, self.camera1_sub, self.camera2_sub, self.camera3_sub], @@ -28,7 +29,7 @@ def __init__(self): 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') + self.vis_publisher = self.create_publisher(Image, '/annotated_img', 10) def process_img(self, image): try: 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..81bb91ef --- /dev/null +++ b/src/perception/camera_object_detection/config/post_synchronizer_config.yaml @@ -0,0 +1,4 @@ +post_synchronizer_node: + ros__parameters: + camera_topic: /camera/right/image_color + publish_vis_topic: /annotated_img 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..606aeaf0 --- /dev/null +++ b/src/perception/camera_object_detection/launch/include/post_synchronizer.launch.py @@ -0,0 +1,21 @@ +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' + ) + + camera_object_detection_node = Node( + package='camera_object_detection', + executable='camera_sync_node', + name='post_synchronizer_node', + parameters=[config] + ) + + return LaunchDescription([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' ], }, )