學習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代價地圖

Nginx R31 doc 官方文檔-01-nginx 如何安裝

挑戰程序設計競賽 2.2章習題 POJ - 3617 Best Cow Line 貪心

golang開發環境搭建(win10)

Toybrick RK3399Pro安裝ROS系統

學習costmap代價地圖

Linux 下 platform 設備驅動

ROS_Dynamic Reconfig 動態參數調節

Ros導航虛擬牆設置

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

linux以太網驅動總結