From 5c746829db2c3819f71957eb21920db92a1236d1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=99=8C=E6=AC=A2=E9=86=89?= <264854363@qq.com> Date: Thu, 4 Jun 2026 22:07:35 +0800 Subject: [PATCH] =?UTF-8?q?=E7=9B=AE=E5=89=8D=E6=AD=A3=E5=9C=A8=E5=B0=9D?= =?UTF-8?q?=E8=AF=95=E4=BB=BF=E7=9C=9F=E4=B8=AD=E5=AE=9E=E7=8E=B0=E5=9F=BA?= =?UTF-8?q?=E7=A1=80=E5=9C=B0=E5=9B=BE=E4=B8=8A=E5=AE=9E=E6=97=B6=E6=9B=B4?= =?UTF-8?q?=E6=96=B0=E5=9C=B0=E5=9B=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/mapper_params_online_async.yaml | 79 --------- .../config/slam_toolbox_async.yaml | 82 +++++++++ .../config/slam_toolbox_default.rviz | 137 --------------- .../launch/gc_nav2_with_amcl.launch.py | 104 ++++++++++-- .../launch/gc_nav2_with_slam.launch.py | 104 ++++++++++-- .../launch/gc_nav2_with_slam_online.launch.py | 158 ++++++++++++++++++ 6 files changed, 419 insertions(+), 245 deletions(-) delete mode 100644 src/gc_navigation2_slamtoolbox/config/mapper_params_online_async.yaml create mode 100644 src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml delete mode 100644 src/gc_navigation2_slamtoolbox/config/slam_toolbox_default.rviz create mode 100644 src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py diff --git a/src/gc_navigation2_slamtoolbox/config/mapper_params_online_async.yaml b/src/gc_navigation2_slamtoolbox/config/mapper_params_online_async.yaml deleted file mode 100644 index 7353303..0000000 --- a/src/gc_navigation2_slamtoolbox/config/mapper_params_online_async.yaml +++ /dev/null @@ -1,79 +0,0 @@ -slam_toolbox: - ros__parameters: - - # Plugin params - solver_plugin: solver_plugins::CeresSolver - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - ceres_preconditioner: SCHUR_JACOBI - ceres_trust_strategy: LEVENBERG_MARQUARDT - ceres_dogleg_type: TRADITIONAL_DOGLEG - ceres_loss_function: None - - # ROS Parameters - odom_frame: odom - map_frame: map - base_frame: base_footprint - scan_topic: /scan - use_map_saver: true - mode: localization #localization - - # if you'd like to immediately start continuing a map at a given pose - # or at the dock, but they are mutually exclusive, if pose is given - # will use pose - # map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph - # map_start_pose: [0.0, 0.0, 0.0] - #map_start_at_dock: true - - debug_logging: false - throttle_scans: 1 - transform_publish_period: 0.02 #if 0 never publishes odometry - map_update_interval: 5.0 - resolution: 0.05 - restamp_tf: false - min_laser_range: 0.0 #for rastering images - max_laser_range: 20.0 #for rastering images - minimum_time_interval: 0.5 - transform_timeout: 0.2 - tf_buffer_duration: 30.0 - stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps - enable_interactive_mode: true - - # General Parameters - use_scan_matching: true - use_scan_barycenter: true - minimum_travel_distance: 0.5 - minimum_travel_heading: 0.5 - check_min_dist_and_heading_precisely: false - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 - link_scan_maximum_distance: 1.5 - loop_search_maximum_distance: 3.0 - do_loop_closing: true - loop_match_minimum_chain_size: 10 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 - loop_match_minimum_response_fine: 0.45 - - # Correlation Parameters - Correlation Parameters - correlation_search_space_dimension: 0.5 - correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 - - # Correlation Parameters - Loop Closure Parameters - loop_search_space_dimension: 8.0 - loop_search_space_resolution: 0.05 - loop_search_space_smear_deviation: 0.03 - - # Scan Matcher Parameters - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 - - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 - minimum_angle_penalty: 0.9 - minimum_distance_penalty: 0.5 - use_response_expansion: true - min_pass_through: 2 - occupancy_threshold: 0.1 diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml new file mode 100644 index 0000000..671fe8c --- /dev/null +++ b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml @@ -0,0 +1,82 @@ +slam_toolbox: + ros__parameters: + + # ============================ Solver 插件参数 ====================================== + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ============================ ROS 基础参数 ========================================= + odom_frame: odom # 里程计坐标系 + map_frame: map # 地图坐标系 + base_frame: base_link # 机器人基座坐标系 + scan_topic: /scan # 激光雷达话题 + use_map_saver: true # 启用地图保存功能 + mode: mapping # mapping = 定位 + 实时建图（地图可更新） + + # ============================ 已有地图加载（在其基础上继续更新）========================== + # 启动时暂不加载（注释掉），通过服务在启动后手动加载： + # ros2 service call /slam_toolbox/deserialize_map slam_toolbox/srv/DeserializePoseGraph "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph', match_type: 1}" + # map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph + map_start_pose: [0.0, 0.0, 0.0] # 初始位姿 [x, y, yaw]（map 坐标系下） + # map_start_at_dock: true # 或从 docking 位姿启动（与 pose 互斥） + + # ============================ 调试与性能 =========================================== + debug_logging: false # 调试日志（生产环境关闭） + throttle_scans: 1 # 每 N 帧激光处理一次（1=全部处理） + transform_publish_period: 0.02 # TF 发布周期（秒），0=不发布里程计 + map_update_interval: 3.0 # /map 话题更新间隔（秒），越小越实时 + resolution: 0.05 # 地图分辨率（米/像素） + restamp_tf: false # 是否重新打时间戳 + min_laser_range: 0.0 # 激光最小有效距离（米） + max_laser_range: 20.0 # 激光最大有效距离（米） + minimum_time_interval: 0.5 # 最小处理时间间隔（秒） + transform_timeout: 0.2 # TF 查找超时（秒） + tf_buffer_duration: 30.0 # TF 缓冲区时长（秒） + stack_size_to_use: 40000000 # 栈大小（序列化大地图需要） + enable_interactive_mode: true # 交互模式：允许外部工具编辑地图 + + # ============================ 通用建图参数 ========================================= + use_scan_matching: true # 启用扫描匹配 + use_scan_barycenter: true # 使用扫描重心 + minimum_travel_distance: 0.5 # 最小移动距离触发处理（米） + minimum_travel_heading: 0.5 # 最小转向角度触发处理（弧度） + check_min_dist_and_heading_precisely: false + scan_buffer_size: 10 # 扫描缓冲区大小（越大匹配越稳定） + scan_buffer_maximum_scan_distance: 10.0 # 缓冲区最大扫描距离 + link_match_minimum_response_fine: 0.1 # 精细匹配最小响应 + link_scan_maximum_distance: 1.5 # 链接扫描最大距离 + + # ============================ 回环检测参数 ========================================= + do_loop_closing: true # 启用回环检测（消除累积误差） + loop_match_minimum_chain_size: 10 # 最小回环链大小 + loop_match_maximum_variance_coarse: 3.0 # 粗匹配最大方差 + loop_match_minimum_response_coarse: 0.35 # 粗匹配最小响应 + loop_match_minimum_response_fine: 0.45 # 精细匹配最小响应 + loop_search_maximum_distance: 3.0 # 回环搜索最大距离 + + # ============================ 相关性匹配 - 扫描匹配参数 ============================== + correlation_search_space_dimension: 0.5 # 搜索空间维度 + correlation_search_space_resolution: 0.01 # 搜索空间分辨率 + correlation_search_space_smear_deviation: 0.1 # 搜索空间平滑偏差 + + # ============================ 相关性匹配 - 回环检测参数 ============================== + loop_search_space_dimension: 8.0 # 回环搜索空间维度 + loop_search_space_resolution: 0.05 # 回环搜索分辨率 + loop_search_space_smear_deviation: 0.03 # 回环搜索平滑偏差 + + # ============================ 扫描匹配器参数 ======================================== + distance_variance_penalty: 0.5 # 距离方差惩罚 + angle_variance_penalty: 1.0 # 角度方差惩罚 + + fine_search_angle_offset: 0.00349 # 精细搜索角度偏移 + coarse_search_angle_offset: 0.349 # 粗搜索角度偏移 + coarse_angle_resolution: 0.0349 # 粗搜索角度分辨率 + minimum_angle_penalty: 0.9 # 最小角度惩罚 + minimum_distance_penalty: 0.5 # 最小距离惩罚 + use_response_expansion: true # 使用响应扩展 + min_pass_through: 2 # 最小通过次数 + occupancy_threshold: 0.1 # 占据阈值 diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_default.rviz b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_default.rviz deleted file mode 100644 index c079a07..0000000 --- a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_default.rviz +++ /dev/null @@ -1,137 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 154 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: slam_toolbox::SlamToolboxPlugin - Name: SlamToolboxPlugin -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 10 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.785398006439209 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 846 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000217000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000125000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000168000001850000018500ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000017e000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - SlamToolboxPlugin: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1200 - X: 72 - Y: 60 diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py index edf5436..a20c1b7 100644 --- a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py +++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py @@ -1,3 +1,24 @@ +# ============================================================================ +# gc_nav2_with_amcl.launch.py +# 功能：基于 AMCL（自适应蒙特卡洛定位）的导航启动文件 +# +# 架构： +# Gazebo 仿真环境 +# ├── robot_state_publisher — 发布机器人 TF 树（URDF 模型） +# └── nav2_bringup_launch — Nav2 一体化启动（包含以下子模块）： +# ├── map_server — 加载 my_map.yaml,发布全量静态地图到 /map +# ├── AMCL — 粒子滤波定位,发布 map→odom TF +# │ 需要手动设置 /initialpose 初始位姿 +# ├── planner_server — 全局/局部路径规划器（SmacHybrid） +# ├── controller_server — 路径跟踪控制器（MPPI Ackermann） +# ├── behavior_server — 恢复行为服务器（spin/backup/wait） +# └── bt_navigator — 行为树导航编排器 +# +# 与 SLAM 定位方案的区别： +# AMCL： 粒子滤波定位,需要手动设置初始位姿,地图固定不更新 +# SLAM定位： 激光扫描匹配定位,自动定位,可加载 .posegraph 序列化地图 +# ============================================================================ + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -11,24 +32,46 @@ from launch.actions import ExecuteProcess def generate_launch_description(): + """生成 LaunchDescription,启动 Gazebo + AMCL定位 + Nav2 导航""" ld = LaunchDescription() # =============================1.定位到包的地址============================================================= + # 通过 ament_index 获取各功能包的安装路径 gc_navigation_fish_dir = get_package_share_directory( 'gc_navigation2_slamtoolbox') nav2_bringup_dir = get_package_share_directory('nav2_bringup') origincar_urdf_dir = get_package_share_directory( 'origincar_description') - # =============================2.声明参数，获取配置文件路径=================================================== - # use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间 + # =============================2.声明参数,获取配置文件路径=================================================== + # use_sim_time: 仿真时间开关。Gazebo 下必须为 True, + # 因为仿真环境通过 /clock 话题提供时间,而非系统时间 use_sim_time = LaunchConfiguration('use_sim_time', default='true') + + # map_yaml_path: 全量静态地图 yaml 文件路径 + # 传给 bringup_launch → map_server 加载并发布到 /map map_yaml_path = LaunchConfiguration('map', default=os.path.join( gc_navigation_fish_dir, 'maps', 'my_map.yaml')) + + # nav2_param_path: Nav2 导航栈参数文件（含 AMCL 粒子滤波配置） + # 注意：此处用 gc_navigation_amcl.yaml,包含 AMCL 参数（粒子数、运动模型等） + # SLAM 方案则用 gc_navigation_slam.yaml（不含 AMCL） nav2_param_path = LaunchConfiguration('params_file', default=os.path.join( gc_navigation_fish_dir, 'params', 'gc_navigation_amcl.yaml')) - # =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件============== + # =============================3.声明启动launch文件========================================================== + # bringup_launch.py 是 Nav2 的一体化启动入口,内部自动处理： + # 1. 声明所有 launch 参数（map, slam, use_sim_time, params_file 等） + # 2. 根据 slam 参数决定启动模式： + # slam=False（默认）→ localization_launch（map_server + AMCL） + # slam=True → slam_launch（slam_toolbox 在线建图） + # 3. 启动 navigation_launch（planner + controller + behavior + bt_navigator） + # 4. 通过 RewrittenYaml 实现参数文件中的变量替换 + # + # 传入参数： + # map: 地图 yaml 文件路径（map_server 加载,AMCL 定位） + # use_sim_time: 仿真时间模式（Gazebo 下必须为 true） + # params_file: Nav2 全部节点的参数配置 nav2_bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( [nav2_bringup_dir, '/launch', '/bringup_launch.py']), @@ -38,20 +81,35 @@ def generate_launch_description(): 'params_file': nav2_param_path}.items(), ) - # =============================4.Gazebo仿真设置========================================================== + # robot_name_in_model: Gazebo 中机器人的模型名称,需与 URDF 中一致 robot_name_in_model = 'mycar' + + # default_model_path: 机器人 URDF 模型文件路径 + # 使用 xacro 宏展开生成最终的 URDF default_model_path = os.path.join( origincar_urdf_dir, "urdf", "origincar.urdf") + + # 声明 model 启动参数,支持命令行覆盖：ros2 launch ... model:=/path/to/custom.urdf model = DeclareLaunchArgument( name="model", default_value=default_model_path) + # Gazebo 仿真世界文件路径 gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world') + + # 启动 Gazebo 仿真器进程 + # --verbose: 输出详细日志 + # -s libgazebo_ros_init.so: 加载 ROS ↔ Gazebo 通信初始化插件 + # -s libgazebo_ros_factory.so: 加载模型生成（spawn_entity）插件 start_gazebo_cmd = ExecuteProcess( cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', gazebo_world_path], output='screen') + # 在 Gazebo 世界中生成机器人模型 + # -entity: 模型在 Gazebo 中的实例名（mycar） + # -file: 要加载的 URDF 模型文件 + # 生成后 Gazebo 会为模型创建对应的关节状态话题等 spawn_entity_cmd = Node( package='gazebo_ros', executable='spawn_entity.py', @@ -60,9 +118,16 @@ def generate_launch_description(): output='screen' ) - robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]), - value_type=str) + # robot_description: 将 XACRO/URDF 模型字符串设为 ROS 参数 + # robot_state_publisher 读取此参数发布各连杆的 TF 坐标变换 + robot_description = ParameterValue( + Command(["xacro ", LaunchConfiguration("model")]), value_type=str) + # robot_state_publisher: 发布机器人各关节、连杆的 TF 变换 + # - 订阅 joint_states 话题获取关节角度 + # - 根据 URDF 模型计算 base_link → laser_link, wheel_link 等变换 + # - publish_frequency=30Hz 确保 TF 更新平滑 + # - use_sim_time=True 使用 /clock 仿真时间 robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -70,17 +135,22 @@ def generate_launch_description(): 'use_sim_time': True, 'publish_frequency': 30.0}] ) - # 2.启动 joint_state_publisher 节点发布非固定关节状态 - joint_state_publisher = Node( - package="joint_state_publisher", - executable="joint_state_publisher" - ) + # joint_state_publisher: 发布非固定关节的默认状态 + # 如果 Gazebo 已经发布 joint_states,此节点可以注释掉避免冲突 + # joint_state_publisher = Node( + # package="joint_state_publisher", + # executable="joint_state_publisher" + # ) - ld.add_action(model) - ld.add_action(nav2_bringup_launch) - ld.add_action(start_gazebo_cmd) - ld.add_action(spawn_entity_cmd) - ld.add_action(robot_state_publisher) - # ld.add_action(joint_state_publisher) + # =============================5.将所有 Action 添加到 LaunchDescription ================================== + # 注意：nav2_bringup_launch 必须在 Gazebo 启动之后, + # 否则会出现 /clock 话题未就绪的问题 + # 可通过 TimerAction 添加延迟,或依赖 launch 系统的自动排序 + ld.add_action(model) # 1. 声明模型路径参数 + ld.add_action(nav2_bringup_launch) # 2. 启动 Nav2 一体化（map_server + AMCL + 导航） + ld.add_action(start_gazebo_cmd) # 3. 启动 Gazebo 仿真器 + ld.add_action(spawn_entity_cmd) # 4. 在 Gazebo 中生成机器人 + ld.add_action(robot_state_publisher) # 5. 发布机器人 TF 树 + # ld.add_action(joint_state_publisher) # 6. (可选) 默认关节状态发布 return ld diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py index 7e20adc..a896429 100644 --- a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py +++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py @@ -1,3 +1,22 @@ +# ============================================================================ +# gc_nav2_with_slam.launch.py +# 功能：基于 slam_toolbox 定位的导航启动文件 +# +# 架构： +# Gazebo 仿真环境 +# ├── robot_state_publisher — 发布机器人 TF 树（URDF 模型） +# ├── map_server — 加载 my_map.yaml，发布全量静态地图到 /map +# ├── slam_toolbox — 加载 my_map.posegraph，激光扫描匹配定位 +# │ 发布 map→odom TF，替代 AMCL 的粒子滤波 +# │ ⚠️ /map 话题重映射为 /slam_map，避免冲突 +# └── navigation_launch — Nav2 导航栈（路径规划 + 运动控制） +# global_costmap 从 /map 获取全量静态地图 +# +# 与 AMCL 方案的区别： +# AMCL： 粒子滤波定位，需要手动设置初始位姿 +# SLAM定位： 激光扫描匹配定位，自动根据地图特征定位 +# ============================================================================ + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -11,27 +30,50 @@ from launch.actions import ExecuteProcess def generate_launch_description(): + """生成 LaunchDescription，启动 Gazebo + SLAM定位 + Nav2 导航""" ld = LaunchDescription() # =============================1.定位到包的地址============================================================= + # 通过 ament_index 获取各功能包的安装路径 pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox') nav2_bringup_dir = get_package_share_directory('nav2_bringup') origincar_urdf_dir = get_package_share_directory('origincar_description') # =============================2.声明参数=============================================================== + # use_sim_time: 仿真时间开关。Gazebo 下必须为 True， + # 因为仿真环境通过 /clock 话题提供时间，而非系统时间 use_sim_time = LaunchConfiguration('use_sim_time', default='true') + + # map_yaml_path: 全量静态地图 yaml 文件路径 + # 由 map_server 加载，发布到 /map 供 global_costmap 使用 map_yaml_path = LaunchConfiguration('map', default=os.path.join( pkg_dir, 'maps', 'my_map.yaml')) + + # nav2_param_path: Nav2 导航栈参数文件（不含 AMCL，含 slam_toolbox） nav2_param_path = LaunchConfiguration('params_file', default=os.path.join( pkg_dir, 'params', 'gc_navigation_slam.yaml')) + + # slam_params_file: slam_toolbox 定位模式的参数文件 + # mode: localization — 只定位不建图 + # 包含 solver 配置、激光匹配参数、TF 帧定义等 slam_params_file = os.path.join( pkg_dir, 'config', 'mapper_params_localization.yaml') - # slam_toolbox 定位所需的序列化地图 + + # posegraph_path: slam_toolbox 定位所需的序列化地图文件 + # .posegraph 文件保存了 slam_toolbox 建图时的位姿图 + # 配合 .data 文件，slam_toolbox 可以还原完整地图用于激光匹配 posegraph_path = os.path.join( pkg_dir, 'maps', 'my_map.posegraph') # =============================3.slam_toolbox 定位模式（替代 AMCL）========================================= - # 注意：slam_toolbox 的 /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图 + # localization_slam_toolbox_node: + # - 加载 .posegraph 序列化地图用于激光扫描匹配 + # - 发布 map→odom 坐标变换（替代 AMCL 的粒子滤波定位） + # - 发布 /pose 话题（当前定位位姿） + # + # ⚠️ 关键：将 slam_toolbox 的 /map 重映射为 /slam_map + # 避免覆盖 map_server 发布到 /map 的全量静态地图 + # 确保 global_costmap 的 static_layer 能获取完整地图边界 slam_toolbox_node = Node( package='slam_toolbox', executable='localization_slam_toolbox_node', @@ -44,6 +86,11 @@ def generate_launch_description(): ) # =============================4.map_server 提供全量静态地图给 global_costmap ============================= + # map_server: + # - 加载 my_map.yaml + my_map.pgm 格式的传统静态地图 + # - 发布完整地图到 /map 话题（199×266 像素，分辨率 0.05 m） + # - 这是 global_costmap 静态层的唯一数据来源 + # - 是 lifecycle 节点，需要 lifecycle_manager 激活 map_server_node = Node( package='nav2_map_server', executable='map_server', @@ -53,6 +100,8 @@ def generate_launch_description(): 'use_sim_time': True}], ) + # lifecycle_manager: 管理 map_server 的生命周期（configure → activate） + # autostart=True 表示启动后自动激活 map_server map_server_lifecycle = Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -64,7 +113,14 @@ def generate_launch_description(): ) # =============================5.仅启动 nav2 navigation（不含 localization）================================= - # 因为 slam_toolbox 已经替代了 AMCL，不需要再跑 localization_launch + # 为什么用 navigation_launch 而不是 bringup_launch？ + # bringup_launch 包含 localization_launch（AMCL + map_server） + # 由于 slam_toolbox 已经替代了 AMCL，只需 navigation_launch 的： + # - planner_server — 全局/局部路径规划 + # - controller_server — 路径跟踪控制（MPPI） + # - behavior_server — 行为树（spin/backup/wait 恢复） + # - bt_navigator — 行为树导航编排 + # - costmap 层 — global_costmap + local_costmap navigation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( [nav2_bringup_dir, '/launch', '/navigation_launch.py']), @@ -73,19 +129,34 @@ def generate_launch_description(): 'params_file': nav2_param_path}.items(), ) - # =============================5.Gazebo仿真设置========================================================== + # =============================6.Gazebo仿真设置========================================================== + # robot_name_in_model: Gazebo 中机器人的模型名称，需与 URDF 中一致 robot_name_in_model = 'mycar' + + # default_model_path: 机器人 URDF 模型文件路径 + # 使用 xacro 预处理生成最终的 URDF default_model_path = os.path.join( origincar_urdf_dir, "urdf", "origincar.urdf") + + # 声明 model 启动参数，支持命令行覆盖 URDF 路径 model = DeclareLaunchArgument( name="model", default_value=default_model_path) + # Gazebo 世界文件路径 gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world') + + # 启动 Gazebo 仿真器 + # --verbose: 详细日志输出 + # -s libgazebo_ros_init.so: 加载 ROS-Gazebo 初始化插件 + # -s libgazebo_ros_factory.so: 加载模型生成插件 start_gazebo_cmd = ExecuteProcess( cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', gazebo_world_path], output='screen') + # 在 Gazebo 中生成机器人模型 + # -entity: 模型实例名称 + # -file: URDF 模型文件 spawn_entity_cmd = Node( package='gazebo_ros', executable='spawn_entity.py', @@ -94,9 +165,14 @@ def generate_launch_description(): output='screen' ) + # robot_description: 将 URDF/XACRO 模型转换为 ROS 参数 + # robot_state_publisher 订阅此参数发布 TF 树 robot_description = ParameterValue( Command(["xacro ", LaunchConfiguration("model")]), value_type=str) + # robot_state_publisher: 发布机器人各连杆的 TF 变换 + # base_link → laser_link, wheel_link 等关节状态 + # publish_frequency=30Hz 保证平滑的 TF 更新 robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -104,13 +180,17 @@ def generate_launch_description(): 'use_sim_time': True, 'publish_frequency': 30.0}] ) - ld.add_action(model) - ld.add_action(start_gazebo_cmd) - ld.add_action(spawn_entity_cmd) - ld.add_action(robot_state_publisher) - ld.add_action(map_server_node) - ld.add_action(map_server_lifecycle) - ld.add_action(slam_toolbox_node) - ld.add_action(navigation_launch) + # =============================7.将所有 Action 添加到 LaunchDescription ================================== + # 启动顺序由 Launch 系统自动管理依赖 + ld.add_action(model) # 1. 声明模型参数 + ld.add_action(start_gazebo_cmd) # 2. 启动 Gazebo 仿真器 + ld.add_action(spawn_entity_cmd) # 3. 生成机器人模型 + ld.add_action(robot_state_publisher) # 4. 发布 TF 树 + ld.add_action(map_server_node) # 5. 加载全量静态地图 → /map + ld.add_action(map_server_lifecycle) # 6. 激活 map_server + ld.add_action(slam_toolbox_node) # 7. 启动 SLAM 定位 → map→odom TF + ld.add_action(navigation_launch) # 8. 启动 Nav2 导航栈 return ld + + diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py new file mode 100644 index 0000000..d4c760a --- /dev/null +++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py @@ -0,0 +1,158 @@ +# ============================================================================ +# gc_nav2_with_slam_online.launch.py +# 功能：基于 slam_toolbox 在线异步建图 + 定位的导航启动文件 +# +# 核心能力： +# 1. 启动时加载已有地图（my_map.posegraph），无需从零建图 +# 2. 运行中持续更新地图（新障碍物自动写入 /map） +# 3. 激光扫描匹配定位（替代 AMCL，自动定位） +# 4. 双层避障：obstacle_layer（/scan 瞬时避障）+ static_layer（/map 持久化障碍） +# +# 架构： +# Gazebo 仿真环境 +# ├── robot_state_publisher — URDF → TF 树（base_link→laser_link 等） +# ├── slam_toolbox — async_slam_toolbox_node +# │ ├── 加载 my_map.posegraph + my_map.data +# │ ├── mode: mapping（定位 + 实时建图） +# │ ├── 发布 /map（全量 + 实时更新） +# │ └── 发布 map→odom TF（定位结果） +# └── navigation_launch — Nav2 导航栈 +# ├── global_costmap ← /map（静态层） +# ├── local_costmap ← /scan（障碍层） +# ├── planner_server — SmacHybrid 路径规划 +# ├── controller_server — MPPI Ackermann 控制 +# ├── behavior_server — spin/backup/wait 恢复 +# └── bt_navigator — 行为树编排 +# +# 与其他方案对比： +# gc_nav2_with_amcl.launch.py: AMCL 粒子滤波，地图固定，需手动设初始位姿 +# gc_nav2_with_slam.launch.py: SLAM 定位模式，地图只读，需 map_server +# gc_nav2_with_slam_online.launch.py: SLAM 在线模式，加载已有地图 + 实时更新（本文件） +# ============================================================================ + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch.substitutions import Command +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + """启动 Gazebo + SLAM 在线建图定位 + Nav2 导航""" + ld = LaunchDescription() + + # =============================1.定位到包的地址============================================================= + pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + origincar_urdf_dir = get_package_share_directory('origincar_description') + + # =============================2.声明路径参数=============================================================== + # use_sim_time: 必须为 true，Gazebo 通过 /clock 话题提供仿真时间 + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + + # nav2_param_path: Nav2 导航栈参数（planner/controller/costmap/behavior） + nav2_param_path = LaunchConfiguration('params_file', default=os.path.join( + pkg_dir, 'params', 'gc_navigation_slam.yaml')) + + # slam 基础配置文件（异步建图 + 实时更新） + slam_params_file = os.path.join( + pkg_dir, 'config', 'slam_toolbox_async.yaml') + + # 已有序列化地图（slam_toolbox 格式：.posegraph + .data） + posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph') + + # =============================3.slam_toolbox：在线异步建图 + 定位============================================ + # 选用 online_async_slam_toolbox_node（而非 localization_slam_toolbox_node）的原因： + # - 加载已有 .posegraph 地图后，仍可继续写入新扫描数据 + # - 异步处理：激光数据在独立线程中建图，不阻塞里程计 + # - 适合导航场景：启动时还原已知区域，运行中发现新障碍物自动更新 /map + # + # 参数说明（以下参数已在 mapper_params_online_async.yaml 中配置，此处覆写确保生效）： + # mode: mapping — 定位 + 建图（覆盖 yaml） + # map_file_name — 已在 yaml 中设为源码路径，不在此覆写 + + + # 已有序列化地图（slam_toolbox 格式：.posegraph + .data） + # 注意：此路径仅供文档参考，实际加载路径在 yaml 配置文件中 + posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph') + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[ + slam_params_file, # 基础参数（solver、TF、分辨率、map_file_name 等） + { + 'use_sim_time': True, + 'mode': 'mapping', # 覆盖 yaml：定位 + 建图 + 'map_update_interval': 3.0, # 每 3 秒更新一次 /map + }, + ], + # 无需 remap /map：online_async 发布的就是权威 /map + ) +# Read: Failed to open requested file: / home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph. +# Failed to read file: / home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph. + # =============================4.启动 Nav2 导航栈========================================================== + # 只用 navigation_launch（不含 localization），因为 slam_toolbox 已承担定位 + navigation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/navigation_launch.py']), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path, + }.items(), + ) + + # =============================5.Gazebo 仿真环境========================================================== + robot_name_in_model = 'mycar' + default_model_path = os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf') + model = DeclareLaunchArgument(name='model', default_value=default_model_path) + + # 启动 Gazebo 仿真器（加载 world 文件和 ROS 插件） + gazebo_world_path = os.path.join(origincar_urdf_dir, 'world', 'test.world') + start_gazebo_cmd = ExecuteProcess( + cmd=[ + 'gazebo', '--verbose', + '-s', 'libgazebo_ros_init.so', # ROS ↔ Gazebo 通信初始化 + '-s', 'libgazebo_ros_factory.so', # 模型生成工厂 + gazebo_world_path, + ], + output='screen', + ) + + # 在 Gazebo 中生成机器人 + spawn_entity_cmd = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-entity', robot_name_in_model, '-file', default_model_path], + output='screen', + ) + + # 发布机器人 TF 树 + robot_description = ParameterValue( + Command(['xacro ', LaunchConfiguration('model')]), value_type=str, + ) + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{ + 'robot_description': robot_description, + 'use_sim_time': True, + 'publish_frequency': 30.0, + }], + ) + + # =============================6.组装 LaunchDescription=================================================== + ld.add_action(model) + ld.add_action(start_gazebo_cmd) + ld.add_action(spawn_entity_cmd) + ld.add_action(robot_state_publisher) + ld.add_action(slam_toolbox_node) + ld.add_action(navigation_launch) + + return ld