import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution


def generate_launch_description():
    config_dir = PathJoinSubstitution(
        [FindPackageShare('saltybot_pure_pursuit'), 'config']
    )
    config_file = PathJoinSubstitution([config_dir, 'pure_pursuit_config.yaml'])

    pure_pursuit = Node(
        package='saltybot_pure_pursuit',
        executable='pure_pursuit_node',
        name='pure_pursuit',
        output='screen',
        parameters=[config_file],
        remappings=[
            ('/odom', '/odom'),
            ('/path', '/path'),
            ('/cmd_vel_tracked', '/cmd_vel_tracked'),
        ],
    )

    return LaunchDescription([
        pure_pursuit,
    ])