ROS 教程之 navigation :在 catkin 环境下创建costmap layer plugin

        在做机器人导航的时候,肯定见到过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格式。

最后在rviz中看看global_costmap中有没有加入这个障碍物点。下面是我在实验室环境的地图,红色标记的地方是人为加入的障碍物。按照上面的程序,障碍物应该出现在机器人正前方1m出,下面这个效果是lz自己的程序。

障碍物的膨胀系数,可以用下面的命令进行动态调试:

rosrun rqt_reconfigure rqt_reconfigure


     

     由于网上关于这部分的教程很少,lz也是自己慢慢摸索出来的,查看了很多ros answer,一一列出在下面。同时,这个地址有一个别人创建的行人layer,为你定制自己的移动障碍物层有很好的借鉴意义。


文章为原创,

请注重博主劳动成果,

错误请指出,转载请注明。

                                --白巧克力


reference:

1.http://answers.ros.org/question/83031/move_base-and-costmap_layers/

2.http://answers.ros.org/question/83471/layered-costmap-problem/

3.http://answers.ros.org/question/159166/understanding-costmap_2d-tutorial-updating-master_grid/

4.http://answers.ros.org/question/196377/clearing-free-space-in-costmap-at-a-specific-location/

5.http://answers.ros.org/question/174106/problem-with-subscriber-within-costmap-layer/

6.http://answers.ros.org/question/171396/costmap_pluginsxml-misbehaving/

已标记关键词 清除标记
©️2020 CSDN 皮肤主题: 游动-白 设计师:上身试试 返回首页