配置使用costmap_2d_node

435 阅读2分钟

​ 应用背景:移动机器人导航规划任务中,需要知道障碍物与机器人的距离信息。原始生成(或绘制)的2D栅格地图无法直接得到机器人距离障碍物的信息,我们需要模拟大量的雷达射线,经过大量计算得到这些信息。即使在静态的环境中,机器人的每个运行周期都需要这样的计算过程。costmap_2d_node作为ROS NAVIGATION STACK的重要插件,直接对2D栅格地图中的障碍物进行高斯扩散,使每个栅格都包含障碍物远近的信息(占据值occupied value)。

系统:ubuntu 14.04+ros indigo


1.Inflation 原理

  costmapspec.png 膨胀(inflation)为被占据栅格(障碍物)按照距离向其周边栅格传播的过程。按照栅格对机器人的影响,可分为5类:

  1. “Lethal”代价表明该栅格包含障碍物,如果机器人的中心在此类栅格中,则机器人发生碰撞。

  2. “Inscribed”代价表明某栅格距离机器人小于机器人本体的内切圆,如果机器人中在此类栅格中,则机器人肯定发生碰撞。

  3. “Possibly circumscribed” 代价与“Inscribed”代价相似,但是其依据为机器人本体的外接圆的半径。当机器人在此类栅格中时,很有可能发生碰撞。

  4. “Freespace”代价为零,在此类栅格中,机器人绝对不会发生碰撞。

  5. “Unknown”代价。

除此之外的其它代价都介于“Freespace”代价与"Possibly circumscribed"代价之间。

 

高斯膨胀函数:

exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)

其中,costmap_2d::INSCRIBED_INFLATED_OBSTACLE 为 254. NOTE: 因为 cost_scaling_factor is multiplied 被乘上负号, 所以增大该因子,将减小障碍物对周围栅格的影响。

 

2. costmap_2d_node安装、配置与应用

2.1 安装

sudo apt-get install ros-indigo-costmap-2d

2.2 配置

在指定文件夹下新建文件minimal.yaml,并填充以下内容保存。(其中cost_scaling_factor与inflation_radius很重要)

##################################
#        minimal.yaml            #         
##################################
plugins: []
publish_frequency: 1.0
footprint: [[-1.3, -0.9], [-1.3, 0.9], [1.3, 0.9], [1.3, -0.9]]
rolling_window: false
plugins: 
   - {name: static_layer,            type: "costmap_2d::StaticLayer"}
   - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer:
  enabled:              true
  cost_scaling_factor:  0.5  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     30.0  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true

将要处理的地图图片realmap.pgm放在指定文件夹下。

20190102123512564.png

并新建realmap.yaml,填充以下内容保存。

##########################
#     realmap.pgm        #
##########################
image: realmap.pgm
resolution: 0.350000
origin: [0.00000, 0.0000, 0.0]
negate: 0
occupied_thresh: 0.98
free_thresh: 0.0
mode: "scale"

在指定文件夹下新建launch文件costmap_test.launch,并填充以下内容保存。

<!-- -->

<launch>
 <node name="static_tf0" pkg="tf" type="static_transform_publisher" args="2 0 0 0 0 0 /map /base_link 100" />
 <node name="map_server" pkg="map_server" type="map_server" args="$(find main_launch)/params/realmap.yaml" />
 <rosparam file="$(find main_launch)/params/minimal.yaml" command="load" ns="/costmap_node/costmap" /> 
</launch>

其中,realmap.yaml为map_server发布的全局静态地图。该消息会被costmap_2d_node作为初始化信息,并得到最终的高斯膨胀代价地图。

 

2.3 应用

roslaunch main_launch costmap_test.launch

rosrun costmap_2d costmap_2d_node

得到高斯膨胀后的地图

20190102124042619.png