配置使用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: 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放在指定文件夹下。
并新建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
得到高斯膨胀后的地图