from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
from launch.conditions import UnlessCondition, IfCondition 
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression 
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():

    is_local_test = DeclareLaunchArgument(
        'local_test',
        default_value="False",
        description='Launch with simulated components'
    )

    return LaunchDescription([
        is_local_test,
        IncludeLaunchDescription(
            PathJoinSubstitution([
                FindPackageShare("mg_control"),
                'launch',
                'launch.py'
            ]),
            launch_arguments={
                'local_test': LaunchConfiguration('local_test')
            }.items()
        ),
        IncludeLaunchDescription(
            PathJoinSubstitution([
                FindPackageShare("mg_lidar"),
                'launch',
                'launch.py'
            ]),
            condition=UnlessCondition(LaunchConfiguration('local_test')),
        ),
        Node(
            package="mg_odometry",
            executable="mg_odom_publisher",
            name="mg_odom_publisher",
            condition=UnlessCondition(LaunchConfiguration('local_test')),
            parameters=[{
                'odom': "odom",
                'serial_path': "/dev/ttyACM0",
            }],

            emulate_tty=True,
            output='screen'
        ),
        IncludeLaunchDescription(
            PathJoinSubstitution([
                FindPackageShare("mg_bt"),
                'launch',
                'launch.py'
            ]),
            condition=IfCondition(LaunchConfiguration('local_test')),
        ),
        Node(
            package="mg_planner",
            executable="mg_planner",
            name="mg_planner",
            emulate_tty=True,
            condition=IfCondition(LaunchConfiguration('local_test')),
            output='screen',
        ),
        Node(
            package="mg_navigation",
            executable="mg_nav_server",
            name="mg_nav_server",
            emulate_tty=True,
            output='screen',
        )
    ])