yiliao2026/install/gc_bringup/share/gc_bringup/launch/navigation_slam.launch.py

#!/usr/bin/env python3
# =============================================================================
# gc_bringup — slam_toolbox 定位 + 导航模式
# 使用 slam_toolbox 定位模式替代 AMCL，适用于有 posegraph 的场景
# =============================================================================
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, ExecuteProcess,
                            IncludeLaunchDescription, TimerAction)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    # ============================ 包路径 ============================
    bringup_dir = get_package_share_directory('gc_bringup')
    origincar_dir = get_package_share_directory('origincar_description')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')

    # ============================ 启动参数 ============================
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    use_gazebo = LaunchConfiguration('use_gazebo', default='true')
    use_rviz = LaunchConfiguration('use_rviz', default='true')
    world = LaunchConfiguration('world', default=os.path.join(
        origincar_dir, 'world', 'gc_world.world'))
    map_yaml = LaunchConfiguration('map', default=os.path.join(
        bringup_dir, 'maps', 'my_map.yaml'))

    declare_use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='true')
    declare_use_gazebo = DeclareLaunchArgument('use_gazebo', default_value='true')
    declare_use_rviz = DeclareLaunchArgument('use_rviz', default_value='true')
    declare_world = DeclareLaunchArgument(
        'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'))
    declare_map = DeclareLaunchArgument(
        'map', default_value=os.path.join(bringup_dir, 'maps', 'my_map.yaml'))

    # ============================ 机器人模型 ============================
    default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
    robot_description = ParameterValue(
        Command(['xacro ', default_model_path]), value_type=str)

    # ============================ Gazebo ============================
    start_gazebo = ExecuteProcess(
        condition=IfCondition(use_gazebo),
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
             '-s', 'libgazebo_ros_factory.so', world],
        output='screen'
    )

    spawn_robot = Node(
        condition=IfCondition(use_gazebo),
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'mycar',
                   '-file', default_model_path,
                   '-x', '0.0', '-y', '0.0', '-z', '0.1'],
        output='screen'
    )

    # ============================ TF & 机器人状态 ============================
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[{
            'robot_description': robot_description,
            'use_sim_time': True,
            'publish_frequency': 30.0
        }],
        output='screen'
    )

    joint_state_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        parameters=[{'use_sim_time': True}],
        output='screen'
    )

    # ============================ slam_toolbox 定位模式 ============================
    map_base_path = os.path.join(bringup_dir, 'maps', 'my_map')
    slam_localization_params = os.path.join(
        bringup_dir, 'config', 'mapper_params_localization.yaml')

    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='localization_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[
            slam_localization_params,
            {
                'map_file_name': map_base_path,
                'use_sim_time': True
            }
        ]
    )

    # ============================ Nav2 Bringup (关闭内部定位，用 slam_toolbox) ============================
    nav2_params = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
    nav2_launch_path = os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')

    nav2_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(nav2_launch_path),
        launch_arguments={
            'map': map_yaml,
            'use_sim_time': use_sim_time,
            'params_file': nav2_params,
            'slam': 'False',
            'autostart': 'True',
            'use_composition': 'False',
        }.items(),
    )

    # ============================ RViz2 ============================
    rviz_config = os.path.join(bringup_dir, 'rviz', 'navigation.rviz')
    rviz_node = Node(
        condition=IfCondition(use_rviz),
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config],
        parameters=[{'use_sim_time': True}],
        output='screen'
    )

    # ============================ 组装 ============================
    ld = LaunchDescription()

    ld.add_action(declare_use_sim_time)
    ld.add_action(declare_use_gazebo)
    ld.add_action(declare_use_rviz)
    ld.add_action(declare_world)
    ld.add_action(declare_map)

    ld.add_action(start_gazebo)
    ld.add_action(TimerAction(period=5.0, actions=[spawn_robot]))
    ld.add_action(robot_state_publisher)
    ld.add_action(joint_state_publisher)
    ld.add_action(TimerAction(
        period=8.0,
        actions=[slam_toolbox_node, nav2_bringup, rviz_node]
    ))

    return ld