AMRL_GDC_LOBBY_TRACKING/start_camera.launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode

# From perspective of cameras
LEFT_CAMERA_NAME = 'left'
RIGHT_CAMERA_NAME = 'right'


def generate_launch_description():
    config_path = os.path.join(
        get_package_share_directory('spinnaker_camera_driver'),
        'config',
        'blackfly_s.yaml'
    )

    # ── Container 1: Camera Drivers (component_container_mt) ─────────────
    #
    # Both cameras share a single process because the FLIR Spinnaker SDK
    # requires USB3 cameras to negotiate bandwidth within one process.
    # Using _mt so both capture callbacks can fire on separate threads.
    #
    # Topics published (per camera):
    #   /stereo/{left,right}/image_raw    (sensor_msgs/Image)
    #   /stereo/{left,right}/camera_info  (sensor_msgs/CameraInfo)
    camera_container = ComposableNodeContainer(
        name='camera_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container_mt',
        composable_node_descriptions=[
            # ── Left Blackfly S (serial 25282106) ──
            # namespace='stereo', name='left' → node name creates sub-namespace
            # publishes: /stereo/left/image_raw, /stereo/left/camera_info
            ComposableNode(
                package='spinnaker_camera_driver',
                plugin='spinnaker_camera_driver::CameraDriver',
                name=LEFT_CAMERA_NAME,
                namespace='stereo',
                parameters=[{
                    'parameter_file': config_path,
                    'serial_number': '25282106',
                    'camera_info_url': 'file:///home/sentry/camera_ws/stereoCal/left.yaml',
                }],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
            # ── Right Blackfly S (serial 25235293) ──
            # namespace='stereo', name='right' → node name creates sub-namespace
            # publishes: /stereo/right/image_raw, /stereo/right/camera_info
            ComposableNode(
                package='spinnaker_camera_driver',
                plugin='spinnaker_camera_driver::CameraDriver',
                name=RIGHT_CAMERA_NAME,
                namespace='stereo',
                parameters=[{
                    'parameter_file': config_path,
                    'serial_number': '25235293',
                    'camera_info_url': 'file:///home/sentry/camera_ws/stereoCal/right.yaml',
                }],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
        ],
        output='screen',
    )

    # ── Container 2: Image Processing (component_container_mt) ───────────
    #
    # Debayer + Rectify for both cameras. Grouped together so that
    # debayer→rectify is zero-copy (intra-process) per side, while _mt
    # lets left and right pipelines run concurrently on different threads.
    #
    # Per-side data flow:
    #   image_raw → DebayerNode → image_mono → RectifyNode → image_rect
    #                           ↘ image_color (available for other consumers)
    image_proc_container = ComposableNodeContainer(
        name='image_proc_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container_mt',
        composable_node_descriptions=[
            # ── Left Debayer ──
            # Subscribes: /stereo/left/image_raw   (default topic, matches camera)
            # Publishes:  /stereo/left/image_mono   (grayscale debayered)
            #             /stereo/left/image_color  (color debayered)
            ComposableNode(
                package='image_proc',
                plugin='image_proc::DebayerNode',
                name='debayer',
                namespace='stereo/left',
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
            # ── Left Rectify ──
            # Remap: 'image' → 'image_mono' so it subscribes to debayer output.
            # image_transport auto-derives camera_info from the image topic's
            # namespace: /stereo/left/image_mono → /stereo/left/camera_info ✓
            # Publishes: /stereo/left/image_rect
            ComposableNode(
                package='image_proc',
                plugin='image_proc::RectifyNode',
                name='rectify_mono',
                namespace='stereo/left',
                remappings=[('image', 'image_mono')],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
            # ── Right Debayer ──
            # Subscribes: /stereo/right/image_raw
            # Publishes:  /stereo/right/image_mono, /stereo/right/image_color
            ComposableNode(
                package='image_proc',
                plugin='image_proc::DebayerNode',
                name='debayer',
                namespace='stereo/right',
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
            # ── Right Rectify ──
            # Remap: 'image' → 'image_mono'
            # Subscribes: /stereo/right/image_mono + /stereo/right/camera_info
            # Publishes:  /stereo/right/image_rect
            ComposableNode(
                package='image_proc',
                plugin='image_proc::RectifyNode',
                name='rectify_mono',
                namespace='stereo/right',
                remappings=[('image', 'image_mono')],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
        ],
        output='screen',
    )

    # ── Container 3: Stereo Processing (component_container_mt) ──────────
    #
    # DisparityNode + PointCloudNode isolated from image processing so the
    # heavy stereo matching doesn't starve debayer/rectify callbacks.
    # Zero-copy between disparity → point_cloud within this container.
    #
    # Data flow:
    #   left/image_rect + right/image_rect → DisparityNode → disparity
    #   disparity + left/image_rect → PointCloudNode → points2
    stereo_proc_container = ComposableNodeContainer(
        name='stereo_proc_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container_mt',
        composable_node_descriptions=[
            # ── Disparity ──
            # Subscribes: /stereo/left/image_rect   (from left RectifyNode)
            #             /stereo/left/camera_info   (from left CameraDriver)
            #             /stereo/right/image_rect  (from right RectifyNode)
            #             /stereo/right/camera_info  (from right CameraDriver)
            # Publishes:  /stereo/disparity          (stereo_msgs/DisparityImage)
            ComposableNode(
                package='stereo_image_proc',
                plugin='stereo_image_proc::DisparityNode',
                name='disparity_node',
                namespace='stereo',
                parameters=[{'approximate_sync': True}],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
            # ── Point Cloud ──
            # Remap: left/image_rect_color → left/image_rect because
            #        RectifyNode publishes 'image_rect', not 'image_rect_color'.
            # Subscribes: /stereo/left/image_rect   (remapped from left/image_rect_color)
            #             /stereo/left/camera_info
            #             /stereo/right/camera_info
            #             /stereo/disparity          (from DisparityNode — zero-copy)
            # Publishes:  /stereo/points2            (sensor_msgs/PointCloud2)
            ComposableNode(
                package='stereo_image_proc',
                plugin='stereo_image_proc::PointCloudNode',
                name='point_cloud_node',
                namespace='stereo',
                parameters=[{'approximate_sync': True}],
                remappings=[
                    ('left/image_rect_color', 'left/image_rect'),
                    ('right/image_rect_color', 'right/image_rect'),
                ],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
        ],
        output='screen',
    )

    # ── Static TF: world → left ──────────────────────────────────────────
    #
    # PointCloudNode publishes points2 with frame_id="left" (the left
    # camera's node name). RViz needs a TF path from its fixed frame to
    # "left" — this identity transform provides it. The left↔right
    # extrinsics are encoded in the camera_info P matrices, not in TF.
    tf_publisher = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=[
            '--x', '0', '--y', '0', '--z', '0',
            '--yaw', '0', '--pitch', '0', '--roll', '0',
            '--frame-id', 'world',
            '--child-frame-id', 'left',
        ],
    )

    return LaunchDescription([
        camera_container,
        image_proc_container,
        stereo_proc_container,
        tf_publisher,
    ])