学习costmap代价地图

0x00 什么是costmap代价地图

在机器人进行路径规划时，我们需要明白规划算法是依靠什么在地图上来计算出来一条路径的。依靠的是gmapping扫描构建的一张环境全局地图，但是仅仅依靠一张原始的全局地图是不行的。因为这张地图是静态的，无法随时来更新地图上的障碍物信息。在现实环境中，总会有各种无法预料到的新障碍物出现在当前地图中，或者旧的障碍物现在已经从环境地图中被移除掉了，那么我们就需要来随时更新这张地图。同时由于默认的地图是一张黑白灰三色地图，即只会标出障碍物区域、自由移动区域和未被探索区域。机器人在这样的地图中进行路径规划，会导致规划的路径不够安全，因为我们的机器人在移动时需要与障碍物之间保持一定的安全缓冲距离，这样机器人在当前地图中移动时就更安全了。

costmap简单来说就是为了在这张地图上进行各种加工，方便我们后面进行路径规划而存在的。那具体该如何实现costmap呢？在ROS中使用costmap_2d这个软件包来实现的，该软件包在原始地图上实现了两张新的地图。一个是local_costmap，另外一个就是global_costmap，根据名字大家就可以知道了，两张costmap一个是为局部路径规划准备的，一个是为全局路径规划准备的。无论是local_costmap还是global_costmap，都可以配置多个图层，包括下面几种：

Static Map Layer：静态地图层，基本上不变的地图层，通常都是SLAM建立完成的静态地图。
Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。
Other Layers：你还可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。

0x01 创建stdr_move_base软件包

你可能会纳闷，为什么讲costmap会提到move_base这个软件包呢？这是因为move_base软件包是ROS中自动导航的核心软件包，我们可以查看下面这张图大家就会对此软件包有深刻认识了：

根据move_base的内部逻辑流程图得知，在进行路径规划时costmap是必不可少的。因此我们需要首先创建个stdr_move_base软件包，然后配置costmap相关的参数，这样move_base软件包内的路径规划器才能找到一条合适的路径控制机器人移动到达指定的目的地。

创建好软件包后，接下来就可以来编写launch文件了，命名为stdr_move_base.launch，该launch文件内容如下：

<!--
  FileName: stdr_move_base.launch
  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
  Author: corvin
  Description:
    启动move_base节点，加载各个配置文件。
  History:
    20180528: initial this file.
    20180530: add arg param to rename all kinds of frames,topics,add load params file.
-->
<launch>
  <arg name="odom_frame_id"   default="/map_static"/>
  <arg name="base_frame_id"   default="/robot0"/>
  <arg name="global_frame_id" default="/map"/>

  <arg name="odom_topic"      default="/robot0/odom"/>
  <arg name="cmd_vel_topic"   default="/robot0/cmd_vel"/>
  <arg name="map_topic"       default="/amcl/map"/>

  <node pkg="move_base" type="move_base" name="stdr_move_base" output="screen">
    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find stdr_move_base)/config/local_costmap_params.yaml"  command="load" />
    <rosparam file="$(find stdr_move_base)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find stdr_move_base)/config/dwa_local_planner_params.yaml"    command="load" />
    <rosparam file="$(find stdr_move_base)/config/move_base_params.yaml"            command="load" />
    <rosparam file="$(find stdr_move_base)/config/global_planner_params.yaml"       command="load" />
   
   <param name="global_costmap/global_frame"     value="$(arg global_frame_id)"/>
   <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
   <param name="local_costmap/global_frame"      value="$(arg odom_frame_id)"/>
   <param name="local_costmap/robot_base_frame"  value="$(arg base_frame_id)"/>
   <param name="DWAPlannerROS/global_frame_id"   value="$(arg odom_frame_id)"/>

    <!-- move base default publish cmd to /cmd_vel topic,now remap to /robot0/cmd_vel -->
    <remap from="/cmd_vel" to="$(arg cmd_vel_topic)"/>

    <!-- move_base default subscribe odom topic,now remap to /robot0/odom -->
    <remap from="/odom" to="$(arg odom_topic)"/>

    <!-- move_base default subscribe map topic,now remap to /amcl/map -->
    <remap from="/map" to="$(arg map_topic)"/>
  </node>
</launch>

在启动move_base节点时，可以看到我们首先加载了costmap_common_params.yaml到global_costmap和local_costmap两个命名空间中，因为该配置文件是一个通用的代价地图配置参数，即local_costmap和global_costmap都需要配置的参数。然后下面是local_costmap_params.yaml专门为了局部代价地图配置的参数，global_costmap_params.yaml专门为全局代价地图配置的参数。

0x02 配置costmap_common_params.yaml

在config目录下，创建costmap_common_params.yaml文件，配置的参数如下：

#FileName: costmap_common_params.yaml
#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
#Author: corvin
#Description:
# 代价地图通用参数配置文件,就是全局代价地图和局部代价地图
# 共同都需要配置的参数,各参数意义如下:
# robot_radius: 机器人的半径
#
#History:
# 20180613: initial this file.

robot_radius: 0.2

obstacle_layer:
  enabled: true
  combination_method: 1
  track_unknown_space: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: laser_scan_sensor
  laser_scan_sensor: {
    sensor_frame: /robot0_laser_0,
    data_type: LaserScan,
    topic: /robot0/laser_0,
    marking: true,
    clearing: true
  }

inflation_layer:
  enabled: true
  cost_scaling_factor: 5.0
  inflation_radius: 0.36

static_layer:
  enabled: true

下面来依次解释下各参数的意义，方便大家以后来根据需要来自行修改调试：

robot_radius：设置机器人的半径，单位是米。由于在stdr中机器人是圆形的，所以可以直接设置该参数。如果你的机器人不是圆形的那就需要使用footprint这个参数，该参数是一个列表，其中的每一个座标代表机器人上的一点，设置机器人的中心为[0,0]，根据机器人不同的形状，找到机器人各凸出的座标点即可，具体可参考下图来设置：

obstacle_layer:配置障碍物图层

enabled:是否启用该层

combination_method:只能设置为0或1，用来更新地图上的代价值，一般设置为1;

track_unknown_space:如果设置为false，那么地图上代价值就只分为致命碰撞和自由区域两种，如果设置为true，那么就分为致命碰撞，自由区域和未知区域三种。意思是说假如该参数设置为true的话，就意味着地图上的未知区域也会被认为是可以自由移动的区域，这样在进行全局路径规划时，可以把一些未探索的未知区域也来参与到路径规划，如果你需要这样的话就将该参数设置为false。不过一般情况未探索的区域不应该当作可以自由移动的区域，因此一般将该参数设置为true;

obstacle_range:设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物;

raytrace_range:在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间。

observation_sources:设置导航中所使用的传感器，这里可以用逗号形式来区分开很多个传感器，例如激光雷达，碰撞传感器，超声波传感器等，我们这里只设置了激光雷达;

laser_scan_sensor:添加的激光雷达传感器

sensor_frame:激光雷达传感器的座标系名称;

data_type:激光雷达数据类型;

topic:该激光雷达发布的话题名;

marking:是否可以使用该传感器来标记障碍物;

clearing:是否可以使用该传感器来清除障碍物标记为自由空间;
inflation_layer:膨胀层，用于在障碍物外标记一层危险区域，在路径规划时需要避开该危险区域

enabled:是否启用该层;

cost_scaling_factor:膨胀过程中应用到代价值的比例因子，代价地图中到实际障碍物距离在内切圆半径到膨胀半径之间的所有cell可以使用如下公式来计算膨胀代价：exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1),公式中costmap_2d::INSCRIBED_INFLATED_OBSTACLE目前指定为254，注意：由于在公式中cost_scaling_factor被乘了一个负数，所以增大比例因子反而会降低代价

inflation_radius:膨胀半径，膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。如果机器人经常撞到障碍物就需要增大该值，若经常无法通过狭窄地方就减小该值。

Static_layer:静态地图层，即SLAM中构建的地图层

enabled:是否启用该地图层;

通过下图来认识下为何要设置膨胀层以及意义：

0x03 配置global_costmap_params.yaml

全局代价地图是作为进行全局路径规划时的参考，我们需要在config目录中，创建global_costmap_params.yaml文件，该文件是为全局代价地图配置的参数，具体配置的参数如下：

#FileName: global_costmap_params.yaml
#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
#Author: corvin
#Description:
#  全局代价地图参数配置文件,各参数的意义如下：
#  global_frame:在全局代价地图中的全局座标系;
#  robot_base_frame:机器人的基座标系;
#
#History:
#  20180613: initial this file.
global_costmap:
  global_frame: /map
  robot_base_frame: /robot0
  update_frequency: 0.5
  static_map: true
  rolling_window: false
  transform_tolerance: 1.0
  plugins:
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

下面我们来详细解释下该全局代价地图配置文件中各参数的意义：

global_frame:全局代价地图需要在哪个座标系下运行;
robot_base_frame:在全局代价地图中机器人本体的基座标系，就是机器人上的根座标系。通过global_frame和robot_base_frame就可以计算两个座标系之间的变换，得知机器人在全局座标系中的座标了。
update_frequency:全局代价地图更新频率，一般全局代价地图更新频率设置的比较小;
static_map:配置是否使用map_server提供的地图来初始化，一般全局地图都是静态的，需要设置为true;
rolling_window:是否在机器人移动过程中需要滚动窗口，始终保持机器人在当前窗口中心位置;
transform_tolerance:座标系间的转换可以忍受的最大延时;
plugins:在global_costmap中使用下面三个插件来融合三个不同图层，分别是static_layer、obstacle_layer和inflation_layer，合成一个master_layer来进行全局路径规划。

0x04 配置local_costmap_params.yaml

局部代价地图配置参数所建立的地图主要是为局部路径规划所使用，我们可以在config目录下，创建local_costmap_params.yaml文件，完整内容如下：

#FileName: local_costmap_params.yaml
#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
#Author: corvin
#Description:
#  本地代价地图需要配置的参数，各参数意义如下:
#  global_frame:在本地代价地图中的全局座标系;
#  robot_base_frame:机器人本体的基座标系;
#
#History:
#  20180613: initial this file.

local_costmap:
  global_frame: /map_static
  robot_base_frame: /robot0
  update_frequency: 5.0
  publish_frequency: 3.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.05
  transform_tolerance: 0.5
  plugins:
    - {name: static_layer,        type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

下面来详细解释下每个参数的意义：

global_frame:在局部代价地图中的全局座标系，一般需要设置为odom_frame，但是由于stdr没有这个座标系，我就拿/map_static来代替了;
robot_base_frame:机器人本体的基座标系;
update_frequency:局部代价地图的更新频率;
publish_frequency:局部代价地图的发布频率;
static_map:局部代价地图一般不设置为静态地图，因为需要检测是否在机器人附近有新增的动态障碍物;
rolling_window:使用滚动窗口，始终保持机器人在当前局部地图的中心位置;
width:滚动窗口的宽度，单位是米;
height:滚动窗口的高度，单位是米;

resolution:地图的分辨率，该分辨率可以从加载的地图相对应的配置文件中获取到;

transform_tolerance:局部代价地图中的座标系之间转换的最大可忍受延时;
plugins:在局部代价地图中，不需要静态地图层，因为我们使用滚动窗口来不断的扫描障碍物，所以就需要融合两层地图（inflation_layer和obstacle_layer）即可，融合后的地图用于进行局部路径规划;

获取代价地图上的点的座标点位置和朝向：

我们在启动了move_base节点后就可以使用Rviz中的"2D Nav Goal"功能来获取到地图上座标点的座标和朝向，具体的命令如下：

rostopic echo /move_base_simple/goal

学习costmap代价地图

Toybrick RK3399Pro安裝ROS系統

學習costmap代價地圖

Linux 下 platform 設備驅動

ROS_Dynamic Reconfig 動態參數調節

Ros導航虛擬牆設置

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結