import launch
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration 
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch_ros.substitutions import FindPackageShare
import os

def generate_launch_description():
    """Generate launch description with multiple components."""

    vision_share = FindPackageShare("").find("toid_vision")
    camera_info = os.path.join(vision_share, 'config/camera_info.yaml')

    is_blue_arg = DeclareLaunchArgument(
        name= "is_blue",
        default_value = "True"
    ) 

    is_blue = LaunchConfiguration("is_blue")

    container = ComposableNodeContainer(
            name='vision_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container_mt',
            composable_node_descriptions=[
                ComposableNode(
                    package='toid_vision',
                    plugin='toid::NutDetector',
                    name='nut_detector',
                    parameters= [{
                        'is_blue': is_blue,
                    }]),
                ComposableNode(
                    package='camera_ros',
                    plugin='camera::CameraNode',
                    name='camera',
                    parameters= [{
                        'width': 1640,
                        'height': 1242,
                        'orientation': 180,
                        'camera': 0,
                        'frame_id': 'camera',
                        'camera_info_url': "file://" + camera_info
                    }],
                    )
            ],
            output='screen',
    )

    return launch.LaunchDescription([
        is_blue_arg,
        container
        ])