ROS 导航安装及实现（二十二）

目录

简介

准备工作

 导航实现01_SLAM建图

1.gmapping简介

2.gmapping节点说明

2.1订阅的Topic

2.2发布的Topic

2.3服务

2.4参数

2.5所需的坐标变换

2.6发布的坐标变换

3.gmapping使用 

3.1编写gmapping节点相关launch文件

3.2执行

teleop_twist_keyboard 键盘控制方法

 导航实现02_地图服务

 1.map_server简介

 2.map_server使用之地图保存节点(map_saver)

 2.1map_saver节点说明

2.2地图保存launch文件 

 2.3 保存结果解释

3.map_server使用之地图服务(map_server) 

 3.1map_server节点说明

3.2地图读取 

3.3地图显示

导航实现03_定位

1.amcl简介 

 2.amcl节点说明

 3.1订阅的Topic

 3.2发布的Topic

 3.3服务

3.4调用的服务 

3.5参数 

 3.6坐标变换

3.amcl使用

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

3.2编写测试launch文件

3.3执行

导航实现04_路径规划

 1.move_base简介

 2.move_base节点说明

2.1动作

2.2订阅的Topic

2.3发布的Topic

2.4服务

2.5参数

3.move_base与代价地图 

3.1概念

3.2组成

3.3碰撞算法

 4.move_base使用

4.1launch文件

4.2配置文件

4.2.1costmap_common_params.yaml 

4.2.2global_costmap_params.yaml

4.2.3local_costmap_params.yaml

4.2.4base_local_planner_params

4.2.5参数配置技巧

4.3launch文件集成

4.4测试

 导航与SLAM建图

1.编写launc文件 

 2.测试

---

简介

本节内容主要介绍导航的完整性实现，旨在掌握机器人导航的基本流程，该章涉及的主要内容如下:

• SLAM建图(选用较为常见的gmapping)

• 地图服务(可以保存和重现地图)

• 机器人定位

• 路径规划

• 上述流程介绍完毕，还会对功能进一步集成实现探索式的SLAM建图。

准备工作

请先安装相关的ROS功能包:

• 安装 gmapping 包(用于构建地图):sudo apt install ros-<ROS版本>-gmapping

• 安装地图服务包(用于保存与读取地图):sudo apt install ros-<ROS版本>-map-server

• 安装 navigation 包(用于定位以及路径规划):sudo apt install ros-<ROS版本>-navigation

新建功能包，并导入依赖: gmapping map_server amcl move_base

 

 导航实现01_SLAM建图

 SLAM算法有多种，当前我们选用gmapping，后续会再介绍其他几种常用的SLAM实现

1.gmapping简介

gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图，对应的，gmapping对硬件也有一定的要求:

• 该移动机器人可以发布里程计消息
• 机器人需要发布雷达消息(该消息可以通过水平固定安装的雷达发布，或者也可以将深度相机消息转换成雷达消息)

关于里程计与雷达数据，仿真环境中可以正常获取的，不再赘述，栅格地图如案例所示。

gmapping 安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-gmapping

2.gmapping节点说明

gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

2.1订阅的Topic

tf (tf/tfMessage)

• 用于雷达、底盘与里程计之间的坐标变换消息。

scan(sensor_msgs/LaserScan)

• SLAM所需的雷达信息。

2.2发布的Topic

map_metadata(nav_msgs/MapMetaData)

• 地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。

map(nav_msgs/OccupancyGrid)

• 地图栅格数据，一般会在rviz中以图形化的方式显示。

~entropy(std_msgs/Float64)

• 机器人姿态分布熵估计(值越大，不确定性越大)。

2.3服务

dynamic_map(nav_msgs/GetMap)

• 用于获取地图数据。

2.4参数

~base_frame(string, default:"base_link")

• 机器人基坐标系。

~map_frame(string, default:"map")

• 地图坐标系。

~odom_frame(string, default:"odom")

• 里程计坐标系。

 

~map_update_interval(float, default: 5.0)

• 地图更新频率，根据指定的值设计更新间隔。

~maxUrange(float, default: 80.0)

• 激光探测的最大可用范围(超出此阈值，被截断)。

~maxRange(float)

• 激光探测的最大范围。

.... 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

gmapping - ROS Wikihttp://wiki.ros.org/gmapping

2.5所需的坐标变换

雷达坐标系→基坐标系（相对于雷达的探测距离转换为相对于机器人的）

• 一般由 robot_state_publisher 或 static_transform_publisher 发布。

基坐标系→里程计坐标系

• 一般由里程计节点发布。

2.6发布的坐标变换

地图坐标系→里程计坐标系

• 地图到里程计坐标系之间的变换。

3.gmapping使用 

3.1编写gmapping节点相关launch文件

launch文件编写可以参考 github 的演示 launch文件：https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch

复制并修改如下:

<launch>
<param name="use_sim_time" value="true"/>
    <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>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />
    <!-- 可以保存 rviz 配置并后期直接使用-->
    <!--
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_nav_sum)/rviz/gmapping.rviz"/>
    -->
</launch>

 

 true为使用仿真时间

3.2执行

1.先启动 Gazebo 仿真环境(此过程略)

 

2.然后再启动地图绘制的 launch 文件:

roslaunch 包名 launch文件名

 

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

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

4.在 rviz 中添加组件，显示栅格地图

 

 

 

 

 

teleop_twist_keyboard 键盘控制方法

补充：

 i：前进

<:后退

>:逆时针旋转后退

m:顺时针旋转后退

j：向左转向

l：向右转向

k：停止运动

u:逆时针前进

o:顺时针前进

t：z+

b：z-

 导航实现02_地图服务

 上一节我们已经实现通过gmapping的构建地图并在rviz中显示了地图，不过，上一节中地图数据是保存在内存中的，当节点关闭时，数据也会被一并释放，我们需要将栅格地图序列化到的磁盘以持久化存储，后期还要通过反序列化读取磁盘的地图数据再执行后续操作。在

ROS中，地图数据的序列化与反序列化可以通过 map_server 功能包实现。

 1.map_server简介

map_server功能包中提供了两个节点: map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。

map_server安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-map-server

 2.map_server使用之地图保存节点(map_saver)

 2.1map_saver节点说明

订阅的topic:

map(nav_msgs/OccupancyGrid)

• 订阅此话题用于生成地图文件。

 跟前面slam发布的数据一致

2.2地图保存launch文件 

地图保存的语法比较简单，编写一个launch文件，内容如下:

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

其中 mymap 是指地图的保存路径以及保存的文件名称。

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

测试:

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

 

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

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

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

 2.3 保存结果解释

 

xxx.pgm 本质是一张图片，直接使用图片查看程序即可打开。

 xxx.yaml 保存的是地图的元数据信息，用于描述图片，内容格式如下:

image: /home/rosmelodic/ws02_nav/src/mycar_nav/map/nav.pgm
resolution: 0.050000
origin: [-50.000000, -50.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

 

 

解释:

• image:被描述的图片资源路径，可以是绝对路径也可以是相对路径。

• resolution: 图片分片率(单位: m/像素)。

    相当于刻度尺功能，比如说0.05m一个像素

resolution: 0.1

 会让整个地图变大

因为现在一个像素0.1m大

• origin: 地图中左下像素的二维姿势，为（x，y，偏航），偏航为逆时针旋转（偏航= 0表示无旋转）。

    地图的位姿（相对于rviz中的原点的位姿）

    设置成[0,0,0]就会让原点处于地图（背景板地图不是slam间的地图）右下角

• occupied_thresh: 占用概率大于此阈值的像素被视为完全占用。

    占用阈值

白色可通行区域

黑色是障碍物

蓝灰是未知区域

1.地图上每个像素取值[0,255]白色：255 黑色：0

2.计算比例p=(255-x)/255

  白色：p=0

  黑色：p=1

  灰色： p=0-1

 判断是否是障碍物

p>occupied_thresh

 视为障碍物体

• free_thresh: 占用率小于此阈值的像素被视为完全空闲。

    空闲阈值

p<free_thresh

就是可以自由通行

• negate: 是否应该颠倒白色/黑色自由/占用的语义。

    取反

map_server 中障碍物计算规则:

1. 地图中的每一个像素取值在 [0,255] 之间，白色为 255，黑色为 0，该值设为 x；
2. map_server 会将像素值作为判断是否是障碍物的依据，首先计算比例: p = (255 - x) / 255.0，白色为0，黑色为1(negate为true，则p = x / 255.0)；
3. 根据步骤2计算的比例判断是否是障碍物，如果 p > occupied_thresh 那么视为障碍物，如果 p < free_thresh 那么视为无物。

备注:

• 图片也可以根据需求编辑。

3.map_server使用之地图服务(map_server) 

 3.1map_server节点说明

发布的话题

map_metadata（nav_msgs / MapMetaData）

• 发布地图元数据。

map（nav_msgs / OccupancyGrid）

• 地图数据。

服务

static_map（nav_msgs / GetMap）

• 通过此服务获取地图。

参数

〜frame_id（字符串，默认值：“map”）

• 地图坐标系。

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)

3.3地图显示

注意：

先启动rviz订阅话题/map

再启动map_server的launch文件

 在 rviz 中使用 map 组件可以显示栅格地图：

1.打开rviz 

 

 2.添加插件

导航实现03_定位

 所谓定位就是推算机器人自身在全局地图中的位置，当然，SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中，导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。

1.amcl简介 

AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。

amcl已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-navigation

 2.amcl节点说明

 amcl 功能包中的核心节点是:amcl。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

 3.1订阅的Topic

scan(sensor_msgs/LaserScan)

• 激光雷达数据。

感知周围的环境

tf(tf/tfMessage)

• 坐标变换消息。

相对于雷达的关系转换为相对于坐标系

initialpose(geometry_msgs/PoseWithCovarianceStamped)

• 用来初始化粒子滤波器的均值和协方差。

map(nav_msgs/OccupancyGrid)

• 获取地图数据。

全局地图通过map话题获取

 3.2发布的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)

• 机器人在地图中的位姿估计。

particlecloud(geometry_msgs/PoseArray)

• 位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

tf(tf/tfMessage)

• 发布从 odom 到 map 的转换。

 3.3服务

global_localization(std_srvs/Empty)

• 初始化全局定位的服务。

request_nomotion_update(std_srvs/Empty)

• 手动执行更新和发布更新的粒子的服务。

set_map(nav_msgs/SetMap)

• 手动设置新地图和姿态的服务

3.4调用的服务 

static_map(nav_msgs/GetMap)

• 调用此服务获取地图数据。

3.5参数 

~odom_model_type(string, default:"diff")

• 里程计模型选择: "diff","omni","diff-corrected","omni-corrected" (diff 差速、omni 全向轮)

~odom_frame_id(string, default:"odom")

• 里程计坐标系。

~base_frame_id(string, default:"base_link")

• 机器人极坐标系。

~global_frame_id(string, default:"map")

• 地图坐标系。

.... 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

 3.6坐标变换

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。

• 里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
• amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。

3.amcl使用

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

关于launch文件的实现，在amcl功能包下的example目录已经给出了示例，可以作为参考，具体实现:

roscd amcl
进入amcl的功能包
ls examples

 打开实例

该目录下会列出两个文件: amcl_diff.launch 和 amcl_omni.launch 文件，前者适用于差分移动机器人，后者适用于全向移动机器人，可以按需选择，此处参考前者，新建 launch 文件，复制 amcl_diff.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"/><!-- 添加地图坐标系 -->

  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

 

amcl下的节点amcl 

 

 差速机器人

 对应设置坐标系

3.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 mycar_nav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find mycar_nav)/launch/amcl.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz"/>
</launch>

 

3.3执行

1.先启动 Gazebo 仿真环境(此过程略)；

 可能由于性能问题

找不到map话题

所以改成单独先运行map再运行test 

 

2.启动键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3.启动上一步中集成地图服务、amcl 与 rviz 的 launch 文件；

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

 

 

5.通过键盘控制机器人运动，会发现 posearray 也随之而改变。

导航实现04_路径规划

 毋庸置疑的，路径规划是导航中的核心功能之一，在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能。

 1.move_base简介

move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。如前所述(7.1)move_base主要由全局路径规划与本地路径规划组成。

move_base已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-navigation

 2.move_base节点说明

 move_base功能包中的核心节点是:move_base。为了方便调用，需要先了解该节点action、订阅的话题、发布的话题、服务以及相关参数。

 

 

2.1动作

动作订阅

move_base/goal(move_base_msgs/MoveBaseActionGoal)

• move_base 的运动规划目标。

订阅客户端发送的目标点

move_base/cancel(actionlib_msgs/GoalID)

• 取消目标。

由于突发情况取消导航的接口

动作发布

move_base/feedback(move_base_msgs/MoveBaseActionFeedback)

• 连续反馈的信息，包含机器人底盘坐标。

连续反馈机器人底盘到哪了

move_base/status(actionlib_msgs/GoalStatusArray)

• 发送到move_base的目标状态信息。

通过这个话题可以获取客户端发给了move_base什么消息

move_base/result(move_base_msgs/MoveBaseActionResult)

• 操作结果(此处为空)。

2.2订阅的Topic

move_base_simple/goal(geometry_msgs/PoseStamped)

• 运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)。

2.3发布的Topic

cmd_vel(geometry_msgs/Twist)

• 输出到机器人底盘的运动控制消息。

自主控制机器人的话题（比如说导航结束）

2.4服务

~make_plan(nav_msgs/GetPlan)

• 请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。

获取路线

~clear_unknown_space(std_srvs/Empty)

• 允许用户直接清除机器人周围的未知空间。

蓝灰色部分的空间就是未知空间，调用这个之后就会只有灰色和黑色

~clear_costmaps(std_srvs/Empty)

• 允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。

2.5参数

请参考官网。

 move_base - ROS Wikihttp://wiki.ros.org/move_base

3.move_base与代价地图 

 

3.1概念

机器人导航(尤其是路径规划模块)是依赖于地图的，地图在SLAM时已经有所介绍了，ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率（每个像素映射现实的几米）等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过SLAM构建的地图在导航中是不可以直接使用的，因为：

1. SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息；
2. 在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入...

所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据。

3.2组成

代价地图有两张:

global_costmap(全局代价地图) 和 local_costmap(本地代价地图)，

前者用于全局路径规划，后者用于本地路径规划。

两张代价地图都可以多层叠加,一般有以下层级:

• Static Map Layer：静态地图层，SLAM构建的静态地图。

• Obstacle Map Layer：障碍地图层，传感器感知的障碍物信息。

• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

• Other Layers：自定义costmap。

多个layer可以按需自由搭配。

3.3碰撞算法

在ROS中，如何计算代价值呢？请看下图:

上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。

• 致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；
• 内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；
• 外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；
• 非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
• 自由区域:栅格值为0，此处机器人可以自由通过；
• 未知区域:栅格值为255，还没探明是否有障碍物。

膨胀空间的设置可以参考非自由空间。

 4.move_base使用

路径规划算法在move_base功能包的move_base节点中已经封装完毕了，但是还不可以直接调用，因为算法虽然已经封装了，但是该功能包面向的是各种类型支持ROS的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同....最后可能会导致不同的路径规划结果，那么在调用路径规划节点之前，我们还需要配置机器人参数。具体实现如下:

1. 先编写launch文件模板
2. 编写配置文件
3. 集成导航相关的launch文件
4. 测试

4.1launch文件

关于move_base节点的调用，模板如下:

<launch>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find 功能包)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find 功能包)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find 功能包)/param/base_local_planner_params.yaml" command="load" />
    </node>

</launch>

aunch文件解释:

启动了 move_base 功能包下的 move_base 节点，respawn 为 false，意味着该节点关闭后，不会被重启；clear_params 为 true，意味着每次启动该节点都要清空私有参数然后重新载入；通过 rosparam 会载入若干 yaml 文件用于配置参数，这些yaml文件的配置以及作用详见下一小节内容。

 

 ctrl+f全部替换

4.2配置文件

关于配置文件的编写，可以参考一些成熟的机器人的路径规划实现，比如: turtlebot3，github链接：https://github.com/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_navigation/param，先下载这些配置文件备用。

在功能包下新建 param 目录，复制下载的文件到此目录: costmap_common_params_burger.yaml、local_costmap_params.yaml、global_costmap_params.yaml、base_local_planner_params.yaml，并将costmap_common_params_burger.yaml 重命名为:costmap_common_params.yaml。

配置文件修改以及解释:

 对应新建四个文件夹和文件

因为前两个只是命名空间不同（因为全局代价和本地代价有公用参数比如说机器人尺寸）

4.2.1costmap_common_params.yaml 

 该文件是move_base 在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下:

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

footprint只要给出闭合图形顶点即可

 obstacle_range: 3.0  在3m内加入代价地图

raytrace_range: 3.5 3.5m外清除出代价地图

inflation_radius: 0.2 障碍区域0.2m内为警戒区

scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}可以设置哪个传感器导航哪个构建代价地图

4.2.2global_costmap_params.yaml

该文件用于全局代价地图参数设置:

global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_footprint #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

4.2.3local_costmap_params.yaml

该文件用于局部代价地图参数设置:

local_costmap:
  global_frame: odom #里程计坐标系
  robot_base_frame: base_footprint #机器人坐标系

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

 rolling_window: true 单纯是窗口是否是动态，无所谓

4.2.4base_local_planner_params

基本的局部规划器参数配置，这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0#原地旋转角速度

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

 

 目标公差设置的越大允许的误差越大

# 是否是全向移动机器人
  holonomic_robot: false

这个是留给麦克纳姆轮的

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

 sim_time越长 本地路径规划和全局路径规划就越像

4.2.5参数配置技巧

以上配置在实操中，可能会出现机器人在本地路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况，如何尽量避免这种情形呢？

全局路径规划与本地路径规划虽然设置的参数是一样的，但是二者路径规划和避障的职能不同，可以采用不同的参数设置策略:

• 全局代价地图可以将膨胀半径和障碍物系数设置的偏大一些；
• 本地代价地图可以将膨胀半径和障碍物系数设置的偏小一些。

这样，在全局路径规划时，规划的路径会尽量远离障碍物，而本地路径规划时，机器人即便偏离全局路径也会和障碍物之间保留更大的自由空间，从而避免了陷入“假死”的情形。

 

 把这段改成不同参数放在全局和本地里

 

4.3launch文件集成

如果要实现导航，需要集成地图服务、amcl 、move_base 与 Rviz 等，集成示例如下:

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

    <!-- 运行move_base节点 -->
    <include file="$(find mycar_nav)/launch/path.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mycar_nav)/rviz/nav.rviz" />

</launch>

4.4测试

1.先启动 Gazebo 仿真环境(此过程略)；

2.启动导航相关的 launch 文件；

运行map 

 运行其他组件

3.添加Rviz组件(参考演示结果),可以将配置数据保存，后期直接调用；

全局代价地图与本地代价地图组件配置如下:

点击该按钮选择目标点 

 

 然后点击地图一点作为目标，拖动粉红色箭头作为运动方向

 添加全局地图（这就用到了之前的命名空间，用同一参数启动两个话题）

 

 添加本地代价地图

 添加全局路径规划

 添加局部路径规划

 

 可以在移动过程中添加障碍物

会自动避障并检测到障碍物加入代价地图

 导航与SLAM建图

场景:在 7.2.1 导航实现01_SLAM建图中，我们是通过键盘控制机器人移动实现建图的，而后续又介绍了机器人的自主移动实现，那么可不可以将二者结合，实现机器人自主移动的SLAM建图呢？

上述需求是可行的。虽然可能会有疑问，导航时需要地图信息，之前导航实现时，是通过 map_server 包的 map_server 节点来发布地图信息的，如果不先通过SLAM建图，那么如何发布地图信息呢？SLAM建图过程中本身就会时时发布地图信息，所以无需再使用map_server，SLAM已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM本身也是可以实现定位的。

该过程实现比较简单，步骤如下:

1. 编写launch文件，集成SLAM与move_base相关节点；
2. 执行launch文件并测试。

1.编写launc文件 

 当前launch文件实现，无需调用map_server的相关节点，只需要启动SLAM节点与move_base节点，示例内容如下:

<launch>
    <!-- 启动SLAM节点 -->
    <include file="$(find mycar_nav)/launch/slam.launch" />
    <!-- 运行move_base节点 -->
    <include file="$(find mycar_nav)/launch/path.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mycar_nav)/rviz/nav.rviz" />
</launch>

 因为nav01_slam里包含rviz实现了所以往nav01_slam添加rvizi配置读取并删除原配置读取

 

删除nav07_slam_auto里的rviz启动

 

 2.测试

1.首先运行gazebo仿真环境；

 

2.然后执行launch文件；

 

3.在rviz中通过2D Nav Goal设置目标点，机器人开始自主移动并建图了；

 

 

4.最后可以使用 map_server 保存地图。

 启动这个就能保存了