第 8 章 机器人实体导航实现_SLAM建图+地图服务+定位(自学二刷笔记)

重要参考：

课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ

讲义链接:Introduction · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

9.3.2 导航实现02_SLAM建图

关于建图实现，仍然选用第7章中学习的：gmapping，实现如下：

2.1编写gmapping节点相关launch文件

在上一节创建的导航功能包中新建 launch 目录，并新建launch文件（文件名自定义，比如： gmapping.launch），代码示例如下：

<launch>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/>
      <param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
      <param name="odom_frame" value="odom"/> <!--里程计坐标系-->
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>
</launch>

关键代码解释：

<remap from="scan" to="scan"/><!-- 雷达话题 -->
<param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
<param name="odom_frame" value="odom"/> <!--里程计坐标系-->

2.2执行

1.执行相关launch文件，启动机器人并加载机器人模型：roslaunch mycar_start start.launch；

2.启动地图绘制的 launch 文件：roslaunch nav gmapping.launch；

3.启动键盘键盘控制节点，用于控制机器人运动建图：rosrun teleop_twist_keyboard teleop_twist_keyboard.py

4.在 rviz 中添加地图显示组件，通过键盘控制机器人运动，同时，在rviz中可以显示gmapping发布的栅格地图数据了，该显示结果与仿真环境下类似。下一步，还需要将地图单独保存。

 

9.3.3 导航实现03_地图服务

可以通过 map_server 实现地图的保存与读取。

3.1地图保存launch文件

首先在自定义的导航功能包下新建 map 目录，用于保存生成的地图数据。地图保存的语法比较简单，编写一个launch文件，内容如下：

<launch>
    <arg name="filename" value="$(find nav)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

其中通过 filename 指定了地图的保存路径以及保存的文件名称。

SLAM建图完毕后，执行该launch文件即可。

测试:

首先，参考上一节，依次启动仿真环境，键盘控制节点与SLAM节点；

然后，通过键盘控制机器人运动并绘图；

最后，通过上述地图保存方式保存地图。

结果：在指定路径下会生成两个文件，xxx.pgm 与 xxx.yaml

3.2地图读取

通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件如下：

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
</launch>

其中参数是地图描述文件的资源路径，执行该launch文件，该节点会发布话题：map(nav_msgs/OccupancyGrid)，最后，在 rviz 中使用 map 组件可以显示栅格地图。

 

9.3.4 导航实现04_定位

在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。

4.1编写amcl节点相关的launch文件

<launch>
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 -->
    <param name="odom_alpha5" value="0.1"/>
    <param name="transform_tolerance" value="0.2" />
    <param name="gui_publish_rate" value="10.0"/>
    <param name="laser_max_beams" value="30"/>
    <param name="min_particles" value="500"/>
    <param name="max_particles" value="5000"/>
    <param name="kld_err" value="0.05"/>
    <param name="kld_z" value="0.99"/>
    <param name="odom_alpha1" value="0.2"/>
    <param name="odom_alpha2" value="0.2"/>
    <!-- translation std dev, m -->
    <param name="odom_alpha3" value="0.8"/>
    <param name="odom_alpha4" value="0.2"/>
    <param name="laser_z_hit" value="0.5"/>
    <param name="laser_z_short" value="0.05"/>
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/>
    <param name="laser_sigma_hit" value="0.2"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_model_type" value="likelihood_field"/>
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d" value="0.2"/>
    <param name="update_min_a" value="0.5"/>

    <param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
    <param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
    <param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

  </node>
</launch>

4.2编写测试launch文件

amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件，内容示例如下：

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find nav)/launch/amcl.launch" />

</launch>

当然，launch文件中地图服务节点和amcl节点中的包名、文件名需要根据自己的设置修改。

4.3执行

1.执行相关launch文件，启动机器人并加载机器人模型：roslaunch mycar_start start.launch；

2.启动键盘控制节点：rosrun teleop_twist_keyboard teleop_twist_keyboard.py；

3.启动上一步中集成地图服务、amcl 的 launch 文件：roslaunch nav test_amcl.launch；

4.启动rviz并添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；

5.通过键盘控制机器人运动，会发现 posearray 也随之而改变。运行结果与仿真环境下类似。