#!/usr/bin/env python3 # ============================================================================= # gc_bringup — AMCL 导航模式 (直接启动 Nav2 节点) # 启动 Gazebo + 机器人 + Nav2 (AMCL 定位) # ============================================================================= import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, ExecuteProcess, TimerAction) from launch.conditions import IfCondition 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') # ==================== 参数 ==================== use_sim_time = LaunchConfiguration('use_sim_time', default='true') use_gazebo = LaunchConfiguration('use_gazebo', default='true') use_rviz = LaunchConfiguration('use_rviz', default='false') 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='false') 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') # ==================== Nav2 参数文件路径 ==================== nav2_params = os.path.join(bringup_dir, 'config', 'nav2_params.yaml') # ==================== Map Server ==================== map_server = Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', parameters=[nav2_params, {'use_sim_time': True, 'yaml_filename': map_yaml}], ros_arguments=['-p', 'use_sim_time:=True'], ) # ==================== AMCL ==================== amcl = Node( package='nav2_amcl', executable='amcl', name='amcl', output='screen', parameters=[nav2_params, {'use_sim_time': True}], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], ros_arguments=['-p', 'use_sim_time:=True'], ) # ==================== Planner Server ==================== planner_server = Node( package='nav2_planner', executable='planner_server', name='planner_server', output='screen', parameters=[nav2_params, {'use_sim_time': True}], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], ros_arguments=['-p', 'use_sim_time:=True'], ) # ==================== Controller Server ==================== controller_server = Node( package='nav2_controller', executable='controller_server', name='controller_server', output='screen', parameters=[nav2_params, {'use_sim_time': True}], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], ros_arguments=['-p', 'use_sim_time:=True'], ) # ==================== Behavior Server ==================== behavior_server = Node( package='nav2_behaviors', executable='behavior_server', name='behavior_server', output='screen', parameters=[nav2_params, {'use_sim_time': True}], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], ros_arguments=['-p', 'use_sim_time:=True'], ) # ==================== BT Navigator ==================== bt_navigator = Node( package='nav2_bt_navigator', executable='bt_navigator', name='bt_navigator', output='screen', parameters=[nav2_params, {'use_sim_time': True}], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], ros_arguments=['-p', 'use_sim_time:=True'], ) # ==================== Waypoint Follower ==================== waypoint_follower = Node( package='nav2_waypoint_follower', executable='waypoint_follower', name='waypoint_follower', output='screen', parameters=[nav2_params, {'use_sim_time': True}], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], ros_arguments=['-p', 'use_sim_time:=True'], ) # ==================== Lifecycle Manager (map + amcl) ==================== lifecycle_localization = Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', output='screen', parameters=[{'use_sim_time': True, 'autostart': True, 'node_names': ['map_server', 'amcl']}], ) # ==================== Lifecycle Manager (nav nodes) ==================== lifecycle_navigation = Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen', parameters=[{'use_sim_time': True, 'autostart': True, 'node_names': ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator', 'waypoint_follower']}], ) # ==================== 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=[ map_server, amcl, planner_server, controller_server, behavior_server, bt_navigator, waypoint_follower, lifecycle_localization, lifecycle_navigation, rviz_node, ])) return ld