"""Launch camera drivers only (no rectification or stereo processing)."""

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

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'
    )

    # ── 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.
    #
    # 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=[
            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}],
            ),
            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',
    )

    # ── Static TF: world -> left ─────────────────────────────────────────
    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,
        tf_publisher,
    ])