# USAGE:
#   ros2 launch odin_ros_driver odin1_driver_ros2.launch.py
#   ros2 launch odin_ros_driver odin1_reprojection_ros2.launch.py
#   ros2 launch odin_ros_driver odin1_ros2.launch.py
import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    package_dir = get_package_share_directory('odin_ros_driver')
    launch_dir = os.path.join(package_dir, 'launch')

    config_file_arg = DeclareLaunchArgument(
        'config_file',
        default_value=os.path.join(package_dir, 'config', 'control_command.yaml'),
        description='Path to the control config YAML file'
    )

    driver_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'odin1_driver_ros2.launch.py')
        ),
        launch_arguments={
            'config_file': LaunchConfiguration('config_file')
        }.items()
    )

    reprojection_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'odin1_reprojection_ros2.launch.py')
        ),
        launch_arguments={
            'config_file': LaunchConfiguration('config_file')
        }.items()
    )

    ld = LaunchDescription()
    ld.add_action(SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1'))
    ld.add_action(config_file_arg)
    ld.add_action(driver_launch)
    ld.add_action(reprojection_launch)
    return ld