Costmap(代价地图) Costmap是机器人收集传感器信息建立和更新的二维或三维地图,可以从下图简要了解。
上图中,红色部分代表costmap中的障碍物,蓝色部分表示通过机器人内切圆半径膨胀出的障碍(注意是机器人内切圆形状),红色多边形是footprint(脚印)(机器人轮廓的垂直投影)。
机器人避障的规则:为了避免碰撞,footprint不应该和红色部分有交叉,机器人中心不应该与蓝色部分有交叉。
代价地图结构形式: ROS的代价地图(costmap)采用网格(grid)形式,每个网格的值(cell cost)从0~255。分成三种状态:被占用(有障碍)、自由区域(无障碍)、未知区域;
具体状态和值对应有下图:
上图可分为五部分,其中红色多边形区域为机器人的轮廓:
(1) Lethal(致命的):机器人的中心与该网格的中心重合,此时机器人必然与障碍物冲突。
(2) Inscribed(内切):网格的外切圆与机器人的轮廓内切,此时机器人也必然与障碍物冲突。
(3)Possibly circumscribed(外切):网格的外切圆与机器人的轮廓外切,此时机器人相当于靠在障碍物附近,所以不一定冲突。
(4) Freespace(自由空间):没有障碍物的空间。
(5) Unknown(未知):未知的空间。
Code处理逻辑: ROS中costmap_2d这个包提供了一个可以配置的结构维护costmap,其中Costmap通过costmap_2d::Costmap2DROS对象利用传感器数据和静态地图中的信息来存储和更新现实世界中障碍物的信息。costmap_2d::Costmap2DROS为用户提供了纯粹的2维索引,这样可以只通过columns查询障碍物。举个例子来说,一个桌子和一双鞋子在xy平面的相同位置,有不同的Z坐标,在costm_2d::Costmap2DROS目标对应的的costmap中,具有相同的cost值。这旨在帮助规划平面空间。
Costmap由多层组成,例如在costmap_2d包中,1.StaticLayer(静态地图层)是第一层, 2.ObstacleLayer(障碍物层)是第二层,3.InflationLayer(膨胀层)是第三层。 这三层组合成了master map(最终的costmap),供给路线规划模块使用。
最终输出mast map给path planing 模块使用。 Costmap主接口是costmap_2d::Costmap2DROS,它维持了Costmap在ROS中大多数的相关的功能。它用所包含的costmap_2d::LayeredCostmap类来跟踪每一个层。每层使用pluginlib(ROS插件机制)来实例化并添加到LayeredCostmap类的对象中。各个层可以被独立的编译,且允许使用C++接口对costmap做出任意的改变。
Costmap ROS接口: ROS对costmap进行了复杂的封装,提供给用户的主要接口是Costmap2DROS,而真正的地图信息是储存在各个Layer中。下图可以简要说明Costmap的各种接口的关系:
Costmap的ObstacleLayer和StaticLayer都继承于CostmapLayer和Costmap2D,因为它们都有自己的地图,Costmap2D为它们提供存储地图的父类,CostmapLayer为它们提供一些对地图的操作方法。而inflationLayer因为没有维护真正的地图所以只和CostmapLayer一起继承于Layer,Layer提供了操作master map的途径。 LayerdCostmap为Costmap2DROS(用户接口)提供了加载地图层的插件机制,每个插件(即地图层)都是Layer类型的。
Costmap初始化流程: 在navigation的主节点move_base中,建立了两个costmap。其中planner_costmap_ros_是用于全局导航的地图,controller_costmap_ros_是用于局部导航用的地图。下图为costmap的初始化流程。
(1)Costmap初始化首先获得全局坐标系和机器人坐标系的转换(世界坐标系和机器人坐标系相对变化关系)
(2)加载各个Layer,例如StaticLayer,ObstacleLayer,InflationLayer。
(3)设置机器人的轮廓
(4)实例化了一个Costmap2DPublisher来发布可视化数据。
(5)通过一个movementCB函数不断检测机器人是否在运动
(6)开启动态参数配置服务,服务启动了更新map的线程。
Costmap更新 Costmap的更新在mapUpdateLoop线程中实现,此线程分为两个阶段:
阶段一UpdateBounds:这个阶段会更新每个Layer的更新区域,这样在每个运行周期内减少了数据拷贝的操作时间。StaticLayer的Static map只在第一次做更新,Bounds 范围是整张Map的大小,而且在UpdateBounds过程中没有对Static Map层的数据做过任何的更新。ObstacleLayer在这个阶段主要的操作是更新ObstaclesMap层的数据,然后更新Bounds。InflationLayer则保持上一次的Bounds。
阶段二UpdateCosts:这个阶段将各层数据逐一拷贝到Master Map,可以通过下图观察Master Map的生成流程。(图来源于David Lu的《Layered Costmaps for Context-Sensitive Navigation》)
在(a)中,初始有三个Layer和Master costmap,Static Layer和Obstacles Layer维护它们自己的栅格地图,而inflation Layer并没有。为了更新costmap,算法首先在各层上调用自己的UpdateBounds方法(b)。为了决定新的bounds,Obstacles Layer利用新的传感器数据更新它的costmap。然后每个层轮流用UpdateCosts方法更新Master costmap的某个区域,从Static Layer开始(c),然后是Obstacles Layer(d),最后是inflation Layer(e)。
示例是为costmap新创建一层并在该层设置一个伪装障碍点,不让机器人移动到该处。实际应用中比如你不想让机器人去某一个地方但该地方又没有真正障碍,你就可以通过这种方式设置一个伪装障碍。
代码基于rbx1和costmap_2d tutorial基础代码进行必要修改而来,本文主要包括2部分: 1)创建新层插件; 2)将新层插件添加到costmap_2d
1 创建新层插件
1.1 创建工作空间
执行如下命令创建工作空间create_new_layers目录,
1)mkdir costmap2d_tutorial_study_ws
2)cd costmap2d_tutorial_study_ws/ && mkdir src
3)catkin_create_pkg create_new_layers roscpp costmap_2d dynamic_reconfigure std_msgs rospy
1.2 编写插件cpp和header文件
(1)在src/create_new_layers/src目录下添加如下2个cpp文件,创建了2个插件可供调用,我们示例中主要使用grid_layer.cpp,
1)simple_layer.cpp
#include<create_new_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 robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
mark_x_ = robot_x + cos(robot_yaw);
mark_y_ = robot_y + sin(robot_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
2)grid_layer.cpp
#include<create_new_layers/grid_layer.h>
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;
namespace simple_layer_namespace
{
unsigned flag = 0;
GridLayer::GridLayer() {}
void GridLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_);
current_ = true;
default_value_ = NO_INFORMATION;
matchSize();
dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
&GridLayer::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
void GridLayer::matchSize()
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
}
void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
enabled_ = config.enabled;
}
void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
if (flag == 0)
{
flag = 1;
}else
return;
double mark_x = robot_x + cos(robot_yaw), mark_y = robot_y + sin(robot_yaw);
unsigned int mx;
unsigned int my;
if(worldToMap(mark_x, mark_y, mx, my)){
setCost(mx, my,LETHAL_OBSTACLE);
}
*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 GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
master_grid.setCost(i, j, costmap_[index]);
}
}
}
} // end namespace
(2)然后在src/create_new_layers/include/create_new_layers目录下创建2个如下头文件,
1)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 robot_x, double robot_y, double robot_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
2)grid_layer.h
#ifndef GRID_LAYER_H_
#define GRID_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 GridLayer : public costmap_2d::Layer, public costmap_2d::Costmap2D
{
public:
GridLayer();
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_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);
bool isDiscretized()
{
return true;
}
virtual void matchSize();
private:
void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
};
}
#endif
1.3 创建插件描述符文件
在src/create_new_layers/目录下创建插件描述符文件costmap_plugins.xml,
<class_libraries>
<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>
<library path="lib/libgrid_layer">
<class type="simple_layer_namespace::GridLayer" base_class_type="costmap_2d::Layer">
<description>Demo Layer that marks all points that were ever one meter in front of the robot</description>
</class>
</library>
</class_libraries>
1.4 导出插件 在package.xml文件导出插件,供costmap_2d包使用
1.5 编译文件修改 修改CMakeLists.txt文件,
include_directories( include ${catkin_INCLUDE_DIRS} )
add_library(simple_layer src/simple_layer.cpp) add_library(grid_layer src/grid_layer.cpp) 1.6 编译插件并导出目录
(1)在工作空间主目录costmap2d_tutorial_study_ws下,执行命令catkin_make;
(2)导出二进制插件目录供调用,执行命令source costmap2d_tutorial_study_ws/devel/setup.bash
1.7 检查插件是否编译成功
rospack plugins --attrib=plugin costmap_2d
从如下输出内容应该看到costmap_2d包插件已经包含你刚才创建的插件,
frontier_exploration /opt/ros/kinetic/share/frontier_exploration/costmap_plugins.xml create_new_layers /home/xxx/costmap2d_tutorial_study_ws/src/create_new_layers/costmap_plugins.xml costmap_2d /opt/ros/kinetic/share/costmap_2d/costmap_plugins.xml
2 调用新插件
以下我们将调用刚刚创建的新层插件,通过简单修改rbx1示例代码完成,为了完成示例一共修改2个导航配置文件。
2.1 修改rbx1_nav/config/fake/costmap_common_params.yaml
添加obstacles,说明障碍数据来源,同时强调了命名空间
obstacles: observation_sources: scan scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
2.2 修改rbx1_nav/config/fake/global_costmap_params.yaml
本示例我们在全局代价地图配置文件添加如下代码,加入插件规范请参考Configuring Layered Costmaps
plugins: - {name: static_map, type: "costmap_2d::StaticLayer"} - {name: obstacles, type: "costmap_2d::VoxelLayer"} - {name: gridlayer, type: "simple_layer_namespace::GridLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
注意:这里我们假定会发布static map并使用它
2.3 仿真运行
(1)提起fake turtlebot机器人
roslaunch rbx1_bringup fake_turtlebot.launch
(2)提起导航功能包集
roslaunch rbx1_nav fake_amcl.launch map:=test_map.yaml
从终端输出我们可以看到,已经在全局层加入我们创建的新层gridlayer(红框内),绿线以下是local costmap层没有修改依然使用默认3层(有标记pre-hydro)
(3)启动可视化
rosrun rviz rviz -d rospack find rbx1_nav
/amcl.rviz
下图中红色圆形框中即为新创建的障碍点,
(4)查看node与topic关系图
虚拟墙是在turtlebot2那里调包出来修改的,主要在RVIZ上面布置一些虚拟的墙体,并加进去虚拟的激光,好让虚拟的墙体也拥有膨胀区域,机器人到达虚拟墙体能够进行避障。
1、安装好yocs_virtual_sensor的包,这个包需要安装比较多的依赖文件
$ git clone https://github.com/yujinrobot/yujin_ocs.git
$ sudo apt-get install ros-kinetic-ecl-*
$ git clone https://github.com/yujinrobot/yocs_msgs.git
$ sudo apt-get install ros-kinetic-ar-track-alvar
2、编译yovs_virtual_sensor包,这个包需要自己更改CMakeLists.txt文件,不然启动launch文件时候会提示找不到节点,主要更改下面这几句:
## Specify additional locations of header files
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
## Declare a cpp executable
add_executable(virtual_sensor_node src/virtual_sensor_node.cpp)
## Add cmake target dependencies of the executable/library
add_dependencies(virtual_sensor_node yocs_msgs_gencpp)
## Specify libraries to link a library or executable target against
target_link_libraries(virtual_sensor_node ${catkin_LIBRARIES})
#############
## Install ##
#############
install(TARGETS virtual_sensor_node
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
3、配置standalone.launch文件,需要添加yaml文件进去,还有虚拟墙的节点。
<launch>
<arg name="virtual_wall" default="$(find yocs_virtual_sensor)/data/wall_list.yaml"/>
<node name="virtual_sensor" pkg="yocs_virtual_sensor" type="virtual_sensor_node" >
<param name="range_min" value="0.0"/>
<param name="range_max" value="6.0"/>
<param name="frequency" value="10.0"/>
<param name="hits_count" value="3"/>
<param name="global_frame" value="/map"/>
<param name="sensor_frame" value="/base_link"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="base_to_virtual" args="0 0 0 0 0 0 /base_footprint /virtual_laser 50"/>
<node name="wall_publisher" pkg="yocs_virtual_sensor" type="wall_publisher.py" required="true" >
<param name="~filename" value="$(arg virtual_wall)"/>
</node>
</launch>
4、添加好虚拟激光进去costmap.yaml文件,本来一个scan的,现在添加为两个scan。
obstacle_layer:
enabled: true
combination_method: 1
track_unknown_space: true
obstacle_range: 2.5
raytrace_range: 3.0
observation_sources: scan scan1
scan: {
sensor_frame: /laser_link,
data_type: LaserScan,
topic: /scan,
marking: true,
clearing: true
}
scan1: {
sensor_frame: /laser_link,
data_type: LaserScan,
topic: /virtual_sensor_scan,
marking: true,
clearing: false
}
5、修改wall_publisher.py。
#修改
wall_pub = rospy.Publisher('wall_pose_list', WallList, latch = True)
#为
wall_pub = rospy.Publisher('wall_pose_list', WallList, latch = True,queue_size= 10)
6、启动导航包看效果了
在ROS中使用costmap_2d这个软件包来实现costmap,该软件包在原始地图上实现了两张新的地图。一个是local_costmap,另外一个就是global_costmap,根据名字大家就可以知道了,两张costmap一个是为局部路径规划准备的,一个是为全局路径规划准备的。无论是local_costmap还是global_costmap,都可以配置多个图层,包括下面几种:
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
Description:
启动move_base节点,加载各个配置文件。
-->
<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" />
<rosparam file="$(find stdr_move_base)/config/navfn_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
#Description:
# 代价地图通用参数配置文件,就是全局代价地图和局部代价地图
# 共同都需要配置的参数,各参数意义如下:
# robot_radius: 机器人的半径
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
下面来依次解释下各参数的意义,方便大家以后来根据需要来自行修改调试:
obstacle_layer:配置障碍物图层
enabled:是否启用该层
combination_method:只能设置为0或1,用来更新地图上的代价值,一般设置为1;
track_unknown_space:如果设置为false,那么地图上代价值就只分为致命碰撞和自由区域两种,如果设置为true,那么就分为致命碰撞,自由区域和未知区域三种。意思是说假如该参数设置为false的话,就意味着地图上的未知区域也会被认为是可以自由移动的区域,这样在进行全局路径规划时,可以把一些未探索的未知区域也来参与到路径规划,如果你需要这样的话就将该参数设置为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
#Description:
# 全局代价地图参数配置文件,各参数的意义如下:
# global_frame:在全局代价地图中的全局坐标系;
# robot_base_frame:机器人的基坐标系;
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"}
下面我们来详细解释下该全局代价地图配置文件中各参数的意义:
0x04 配置local_costmap_params.yaml
局部代价地图配置参数所建立的地图主要是为局部路径规划所使用,我们可以在config目录下,创建local_costmap_params.yaml文件,完整内容如下:
#FileName: local_costmap_params.yaml
#Description:
# 本地代价地图需要配置的参数,各参数意义如下:
# global_frame:在本地代价地图中的全局坐标系;
# robot_base_frame:机器人本体的基坐标系;
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: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
下面来详细解释下每个参数的意义:
resolution:地图的分辨率,该分辨率可以从加载的地图相对应的配置文件中获取到;
0x05 在RViz中查看global_costmap和local_costmap
我们介绍了半天global_costmap和local_costmap的意义及参数如何设置,但是大家应该还是对它们没有一个直观的感受和认识,我们这里就通过在Rviz中来看看这两种代价地图,如下图所示:
0x06 Bibliography
[1] costmap_2d的ROS Wiki主页. http://wiki.ros.org/costmap_2d/
http://wiki.ros.org/costmap_2d/Tutorials/Creating%20a%20New%20Layer
[2] move_base软件包的ROS Wiki主页. http://wiki.ros.org/move_base
[3] 机器人操作系统入门中关于costmap的介绍. https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter10/10.3.html
[4] costmap_2d软件包学习笔记. https://blog.csdn.net/sonictl/article/details/51518492
[5] costmap_2d之inflation层简介. https://blog.csdn.net/x_r_su/article/details/53420209
此处可能存在不合适展示的内容,页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容,可点击提交进行申诉,我们将尽快为您处理。