"""Launch file for saltybot social navigation."""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument('follow_mode', default_value='shadow',
                              description='Follow mode: shadow/lead/side/orbit/loose/tight'),
        DeclareLaunchArgument('follow_distance', default_value='1.2',
                              description='Follow distance in meters'),
        DeclareLaunchArgument('max_linear_speed', default_value='1.0',
                              description='Max linear speed (m/s)'),
        DeclareLaunchArgument('routes_dir',
                              default_value='/mnt/nvme/saltybot/routes',
                              description='Directory for saved routes'),

        Node(
            package='saltybot_social_nav',
            executable='social_nav',
            name='social_nav',
            output='screen',
            parameters=[{
                'follow_mode': LaunchConfiguration('follow_mode'),
                'follow_distance': LaunchConfiguration('follow_distance'),
                'max_linear_speed': LaunchConfiguration('max_linear_speed'),
                'routes_dir': LaunchConfiguration('routes_dir'),
            }],
        ),

        Node(
            package='saltybot_social_nav',
            executable='midas_depth',
            name='midas_depth',
            output='screen',
            parameters=[{
                'onnx_path': '/mnt/nvme/saltybot/models/midas_small.onnx',
                'engine_path': '/mnt/nvme/saltybot/models/midas_small.engine',
                'process_rate': 5.0,
                'output_scale': 1.0,
            }],
        ),

        Node(
            package='saltybot_social_nav',
            executable='waypoint_teacher',
            name='waypoint_teacher',
            output='screen',
            parameters=[{
                'routes_dir': LaunchConfiguration('routes_dir'),
                'recording_interval': 0.5,
            }],
        ),
    ])
