配置使用costmap_2d_node

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

系统：ubuntu 14.04+ros indigo

1. Inflation 原理

膨胀(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:              truecost_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放在指定文件夹下。

并新建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.launchrosrun costmap_2d costmap_2d_node

得到高斯膨胀后的地图