ROS 教程之 navigation ：在 catkin 環境下創建costmap layer plugin

本文轉載自查看原文 2014-12-29 21:34 3199 用ROS開發自己的機器人

在做機器人導航的時候，肯定見到過global_costmap和local_costmap。global_costmap是為了全局路徑規划服務的，如從這個房間到那個房間該怎么走。local_costmap是為局部路徑規划服務的，如機器人局部有沒有遇到行人之類的障礙。costmap就是柵格圖，每個柵格里存放着障礙物信息。但是hydro里的costmap不是單獨一個柵格圖，而是將一個柵格圖分成了很多層，如最底下是靜態地圖，即牆壁，桌子等建地圖時已經存在的障礙物；第二層是障礙物層；第三層是膨脹層，可以理解為把障礙物膨脹了一圈，最上面就是把前面各層的cost疊加起來形成最后的柵格圖mast_grid，關於這種costmap的簡介見wiki介紹。這些分層的cost layer 是怎么疊加起來，他們是如何工作的？有何優點？可以參見costmap layer 的發明人David V.Lu的IROS會議論文，關於costmap_2d package的細節主要來自這個ros answer。

那么我們自己能用costmap layer 來干什么呢？首先來看一個問題，假設你想讓你的機器人不能去廚房或者其他一些地方，可能你會想在全局地圖中的這些地方人為的放置一些假障礙，那機器人就去不了了。可假障礙如何放到機器人已經生成的costmap里？有人用假激光或者假的點雲數據來制造假障礙，但是，很麻煩。如果你能創建一個專門存放這些假障礙的costmap ，然后把這個costmap作為插件(plugin)放到ROS自帶的地圖里去，這個問題就解決了。

ROS的官網wiki里有教你怎么新建costmap layer以及怎么插入到global_costmap 或者local_costmap里去，官方的tutorials請點擊這里，教程里的例子是在你機器人前方1m處防止一個障礙點。可惜教程是建立在rosbulid編譯環境下的，對於一路都是使用catkin的新手來說，怎么使得rosbuild 和catkin兩個work space合起來工作，都成問題。所以，這里就教你如何在catkin環境下創建layer。

本文分為兩部分：

1、如何在catkin 工作空間中創建New layer 插件

2、如何在程序中調用新創建的New layer

一、創建 layer 插件

在這部分，將創建一個simpler layer，這個layer的作用是在機器人前方1m處放置一個障礙物。

1.首先像創建beginner_tutorials package一樣，在catkin工作空間下創建simple_layers package。在終端中輸入：

cd ~/catkin_ws/src
catkin_create_pkg simple_layers roscpp costmap_2d dynamic_reconfigure std_msgs rospy

其中：catkin_ws/src 對應你自己的catkin工作空間。

2.創建layer 所需的頭文件。

在創建好的simple_layers/include/simple_layers/ 文件夾下創建空白文檔，命名為simple_layer.h，將下列程序復制進去並保存。

#ifndef SIMPLE_LAYER_H_
#define SIMPLE_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/GenericPluginConfig.h>
#include <dynamic_reconfigure/server.h>

namespace simple_layer_namespace
{

class SimpleLayer : public costmap_2d::Layer
{
public:
  SimpleLayer();

  virtual void onInitialize();
  virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x,
                             double* max_y);
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

private:
  void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);

  double mark_x_, mark_y_;
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
};
}
#endif

新建了一個SimpleLayer類，基類是costmap_2d::Layer，可以點擊這里查看costmap_2d類。上面頭文件里有兩個主要的函數updateBounds 和 updateCosts。這個函數對應前面提到的論文中的更新costmap區域和更新costmap的值。updateBounds的參數origin_x,origin_y,origin_yaw是機器人的當前位姿，不需要我們人為的去設定，ROS會自動傳遞這幾個參數。
3.創建c++文件，在simpler_layers/src文件夾下創建空白文檔，命名為simple_layer.cpp，輸入下面程序，並保存。

#include<simple_layers/simple_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;

namespace simple_layer_namespace
{

SimpleLayer::SimpleLayer() {}

void SimpleLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &SimpleLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}


void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
}

void SimpleLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
{
  if (!enabled_)
    return;

  mark_x_ = origin_x + cos(origin_yaw);
  mark_y_ = origin_y + sin(origin_yaw);

  *min_x = std::min(*min_x, mark_x_);
  *min_y = std::min(*min_y, mark_y_);
  *max_x = std::max(*max_x, mark_x_);
  *max_y = std::max(*max_y, mark_y_);
}

void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;
  unsigned int mx;
  unsigned int my;
  if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
    master_grid.setCost(mx, my, LETHAL_OBSTACLE);
  }
}

} // end namespace

這個程序的主要思路是，更新updateBounds中的mark_x_和mark_y_，這兩個變量是用來存放障礙點位置的，他們是在世界坐標系定義的變量。然后在updateCosts里將這兩個變量從世界坐標轉換到地圖坐標系，並在地圖坐標系中設定障礙點，即在柵格地圖master_grid設定這個點的cost。
以上部分都和官方教程中沒什么差別，下面開始是如何在ros中注冊這個插件的關鍵步奏了，這部分可以參看 wiki pluginlib的說明，也可以看看 navigation里如何寫自己的全局路徑規划插件教程。如果，沒有過寫插件的基礎，先照着Lz下面的教程執行一遍，再回過頭去看上面的兩個教程。

4.修改simpler_layers package下的Cmakerlists.txt文件。

添加：

add_library(simple_layer src/simple_layer.cpp)

然后修改在include_directories(...)中添加include，修改后如下：

include_directories(include ${catkin_INCLUDE_DIRS})

添加include的目的是，為了在編譯這個package的時候，能夠找到之前你存放在simple_layers/include/simple_layers這個文件下的頭文件，沒有這個步奏，編譯將出錯。

5.創建一個空白的costmap_plugins.xml文件，輸入一下內容，將其存放在/catkin_ws/src/simpler_layers 文件夾下，也就是和Cmakelists.txt以及package.xml存放在同一路徑下：

<library path="lib/libsimple_layer">
  <class type="simple_layer_namespace::SimpleLayer" base_class_type="costmap_2d::Layer">
    <description>Demo Layer that adds a point 1 meter in front of the robot</description>
  </class>
</library>

這個文件是插件的描述文件，它的目的是讓ROS系統發現這個插件和load這個插件到系統里去。

6.修改package.xml，將下面語句

<costmap_2d plugin="${prefix}/costmap_plugins.xml" />

放入package.xml中兩個<export>之間。效果如圖：

<export>
    <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
  </export>

這一步的目的是注冊這個插件，說白了就是告訴pluginlib，這個插件可用。

上面步奏完成以后，就是編譯了。

cd ~/catkin_ws
catkin_make

編譯完成以后，可以查看ROS系統里是否已經有了這個layer 插件。在終端中輸入：

rospack plugins --attrib=plugin costmap_2d

應該會得到LZ這樣的結果：

說明simple_layer 已經是一個可供ROS用的插件了。

二、如何在move_base中使用這個costmap layer插件

上面已經創建好了layer插件，並不意味着ROS就會使用它，我們得顯式的global_costmap 或者 local_costmap中聲明它。當然首先可以看看wiki 配置layer costmap的教程。看不懂沒關系，至少心里先有個大概印象。

在調用自己寫的這個layer之前，先看看系統默認的global_costmap 和local_costmap使用了哪些layer。假設你已經安裝了《ROS by example 1》中的rbx1 package。

roslaunch rbx1_bringup fake_turtlebot.launch

在新的終端中輸入：

roslaunch rbx1_nav fake_move_base_blank_map.launch

你將看到如下圖所示的一些信息：

你會看到pre-hydro 字樣。說明當前的costmap是默認的配置，也就是static_layer,obstacle_layer,inflation_layer這三層。

下面我們來把創建的simple_layer，放入全局global_costmap中。要想使得ROS接受你的插件，要在參數中用plugins指明，也就是主要修改move_base中涉及到costmap的yaml文件，下面給出我的修改：

1.costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.49
inflation_radius: 0.1
max_obstacle_height: 0.6
min_obstacle_height: 0.0
obstacles:
   observation_sources: scan
   scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

一定要注意，這里添加了一個obstales，說明障礙物層的數據來源scan，"obstacles："的作用是強調命名空間。

2.global_costmap_params.yaml

global_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 1.0
   publish_frequency: 0
   static_map: true
   rolling_window: false
   resolution: 0.01
   transform_tolerance: 1.0
   map_type: costmap
   plugins:
      - {name: static_map,       type: "costmap_2d::StaticLayer"}
      - {name: obstacles,        type: "costmap_2d::VoxelLayer"}
      - {name: simplelayer,        type: "simple_layer_namespace::SimpleLayer"}     
      - {name: inflation_layer,        type: "costmap_2d::InflationLayer"}

3.local_costmap_params.yaml

local_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 3.0
   publish_frequency: 1.0
   static_map: false
   rolling_window: true
   width: 6.0
   height: 6.0
   resolution: 0.01
   transform_tolerance: 1.0
   map_type: costmap

另外一個與路徑規划相關的base_local_planner_params,yaml不用修改。

這里由於只在全局層添加simple_layer，所以local_costmap還是使用的默認layer插件。然后我們運行這個新配置的move_base launch文件，你會發現simplerlayer已經添加進global_costmap了，而local_costmap還是默認的pre-hydro格式。