在 TurtleBot3 上实现同步建图与导航（在线 SLAM）

系统架构

通过同时运行gmapping（建图）和move_base（导航）来实现，但需要注意两者之间的协调，特别是地图的更新和共享。

步骤概述：

1. 修改gmapping_demo.launch文件，使其发布的地图话题与导航使用的地图话题一致，并调整参数以适应动态地图。

2. 修改navigation.launch文件（或创建一个新的融合launch文件），使其同时启动gmapping和move_base，并确保地图服务器使用gmapping实时发布的地图。

3. 注意：在导航中使用动态地图时，需要配置move_base以订阅实时更新的地图。

[Gazebo仿真环境]
     |
     v
[gmapping SLAM节点] <---> [move_base导航节点]
     | 发布动态地图          | 使用动态地图
     v                     v
[map_server动态地图服务]   [全局/局部路径规划]
     |
     v
[RViz可视化]

1. launch启动文件

<launch>
  <!-- TurtleBot3 型号设置 -->
  <arg name="model" default="waffle_pi" doc="model type [burger, waffle, waffle_pi]"/>
  
  <!-- Gazebo 仿真环境 -->
  <include file="$(find turtlebot3_gazebo)/launch/turtlebot3_house.launch">
    <arg name="model" value="$(arg model)" />
  </include>
  
  <!-- 融合的SLAM和导航节点 -->
  <include file="$(find nav)/launch/turtlebot3_gmapping.launch" />
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" />
  
  <node pkg="rviz" type="rviz" name="rviz" required="true"
        args="-d $(find nav)/rviz/online_slam.rviz"/>
</launch>

2. 新建地图配置文件

dynamic_map.yaml

# 动态地图配置
image: dynamic_map.pgm
resolution: 0.05
origin: [0.0, 0.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

3. 修改建图参数

<include file="$(find nav)/launch/turtlebot3_gmapping.launch">
    <arg name="model" value="$(arg model)"/>
    <param name="map_update_interval" value="1.0"/> 
    <param name="delta" value="0.05"/>
</include>
# 各参数含义
<param name="map_update_interval" value="0.5"/>  <!-- 降低地图更新间隔 -->
<param name="linearUpdate" value="0.1"/>         <!-- 线性运动阈值触发更新 -->
<param name="angularUpdate" value="0.1"/>        <!-- 角度运动阈值触发更新 -->
<param name="temporalUpdate" value="0.5"/>       <!-- 时间阈值触发更新 -->
<param name="particles" value="80"/>             <!-- 减少粒子数以提高性能 -->

4. 修改导航参数

  <!-- 导航节点 -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find nav)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find nav)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find nav)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    
    <param name="global_costmap/global_frame" value="map"/>
    <param name="global_costmap/robot_base_frame" value="base_footprint"/>
    <param name="global_costmap/laser_scan_sensor/sensor_frame" value="base_scan"/>
    <param name="global_costmap/laser_scan_sensor/topic" value="scan"/>
    <param name="global_costmap/laser_scan_sensor/data_type" value="LaserScan"/>
    <param name="global_costmap/static_map" value="false" />
    <param name="global_costmap/rolling_window" value="true"/>
    <param name="global_costmap/width" value="10.0" />
    <param name="global_costmap/height" value="10.0" />
    <param name="global_costmap/resolution" value="0.05"/>
    <param name="global_costmap/map_topic" value="/dynamic_map"/>
    
    <param name="local_costmap/global_frame" value="odom"/>
    <param name="local_costmap/robot_base_frame" value="base_footprint"/>
    <param name="local_costmap/laser_scan_sensor/sensor_frame" value="base_scan"/>
    <param name="local_costmap/laser_scan_sensor/topic" value="scan"/>
    <param name="local_costmap/laser_scan_sensor/data_type" value="LaserScan"/>
    <param name="local_costmap/update_frequency" value="10.0"/>
    <param name="local_costmap/publish_frequency" value="5.0"/>
    <param name="local_costmap/static_map" value="false"/>
    <param name="local_costmap/rolling_window" value="true"/>
    <param name="local_costmap/width" value="3.0"/>
    <param name="local_costmap/height" value="3.0"/>
    
    <param name="planner_frequency" value="1.0"/>
    <param name="controller_frequency" value="5.0"/>
    <param name="controller_patience" value="3.0"/>
    <param name="conservative_reset_dist" value="1.0"/>
    <arg name="model" value="$(arg model)" />
  </node>

5. 地图更新

  <!-- 动态地图服务器 -->
  <node pkg="map_server" type="map_server" name="dynamic_map_server" args="/home/suo/catkin_ws/src/maps/dynamic_map.yaml">
    <param name="frame_id" value="map"/>
    <remap from="map" to="dynamic_map"/>
    <remap from="map_metadata" to="dynamic_map_metadata"/>
  </node>

6. 增加机器人本体坐标系变换

  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link"
        args="0.0 0.0 0.0 0.0 0.0 0.0 base_footprint base_link 100"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser_scan"
        args="0.0 0.0 0.0 0.0 0.0 0.0 base_link base_scan 100"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_imu_link"
        args="0.0 0.0 0.0 0.0 0.0 0.0 base_link imu_link 100"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_wheel_right_link"
        args="0.0 0.0 0.0 0.0 0.0 0.0 base_link wheel_right_link 100"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_wheel_left_link"
        args="0.0 0.0 0.0 0.0 0.0 0.0 base_link wheel_left_link 100"/>

7. 编译项目包

# 修改编译配置文件（CMakelists.txt)
install(DIRECTORY scripts launch param rviz
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
cd ~/catkin_ws
catkin_make_isolated --install --pkg nav

8. RViz 配置 (online_slam_nav.rviz)

在 RViz 中添加以下显示项：

• Map: 订阅 /dynamic_map 话题，显示实时更新的地图
• LaserScan: 订阅 /scan 话题
• RobotModel: 显示机器人模型
• Path: 订阅 /move_base/NavfnROS/plan 全局路径
• Path: 订阅 /move_base/DWAPlannerROS/local_plan 局部路径
• PoseArray: 订阅 /particlecloud 显示 gmapping 粒子
• TF: 显示坐标变换

9. 运行

• 启动融合系统:

# 启动
roslaunch nav online_slam.launch
#  启动键盘控制（保持终端激活）
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
# 保存地图
rosrun map_server map_saver -f ./maps/tmp_dynamic_map

• 地图更新优化

# 在gmapping参数中添加
<param name="maxRange" value="5.0"/>  <!-- 限制最大范围减少计算量 -->
<param name="minimumScore" value="100"/> <!-- 提高匹配质量阈值 -->
<param name="ogain" value="1.5"/>     <!-- 优化高斯增益 -->