from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition 
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node, LifecycleNode
from launch_ros.substitutions import FindPackageShare
import os

def generate_launch_description():
    pkg_share = FindPackageShare("").find('toid_navigation')
    #default_model_path = os.path.join(pkg_share, 'src', 'toid_bot_description.urdf')
    params = os.path.join(pkg_share, 'params', 'toid_costmap_params.yaml')
    map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'conf.rviz')


    map_server = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'yaml_filename': map}]
    )

    costmap2d_node = Node(
        package='nav2_costmap_2d',
        executable='nav2_costmap_2d',
        name='global_costmap',
        output='screen',
        parameters=[params]
    )

    lifecycle_manager_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager',
        output='screen',
        parameters=[params]
    )

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', default_rviz_config_path]
    )

    return LaunchDescription([
        map_server,
        costmap2d_node,
        lifecycle_manager_node,
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='map_to_base_link_broadcaster',
            arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link']
        ),
        rviz_node
    ])