Ros导航之代价地图－超声层

在Ros导航里的代价地图，添加超声层，用于躲避障碍，可弥补激光雷达探测不到的透明及镜面的环境

---

前言

Ros的navigation里有Costmap_2d，其中可以添加多种层，本文主要介绍如何添加超声层（描述超声波传感器检测到的障碍物）

---

一、下载地址

github链接
下载Ros对应版本的range_sensor_layer功能包
若为kinetic可以选择indigo版本，(一般高版本也兼容低版本的功能包)

二、使用

1.编译

将解压后的功能包复制到工作空间的src目录下

catkin_make

2.检查

检查插件是否安装成功：

rospack plugins --attrib=plugin costmap_2d

若成功，会出现range_sensor_layer插件，否则只有costmap_2d插件。

3.发布话题、静态TF树

1. 发布话题
   处理超声波数据，并发布消息类型为：sensor_msgs/Range的超声波话题
   sensor_msgs/Range的定义链接
   部分翻译可能不完全正确，但不影响理解
   
2. 发布静态TF树
   在launch文件里发布/base_link和超声波话题里的frame_id
   之间的静态TF树
   在launch发布静态TF树链接

4.配置导航参数文件

1. costmap_common_params.yaml
   topics:[填自己定义的超声波发布话题名]
   笔者有四个超声波，按照图片格式填写话题名

sonar_layer:
  enabled:            true
  clear_threshold:    0.2
  mark_threshold:     0.8
  topics: ["/ultrasonic0", "/ultrasonic1", "/ultrasonic2", "/ultrasonic3"]
  clear_on_max_reading: true

2. local_costmap_params.yaml

- {name: sonar_layer,                 type: "range_sensor_layer::RangeSensorLayer"}

5.运行

正常运行nvigation导航功能包，在rviz中添加Range层，即可看到超声层

一共有四个超声波，只在两个方位放置了障碍物，故只会显示两个。

---

注意事项

• 一定要把sonar_layer(超声层)加在obstacle_layer(障碍层)后面、inflation_layer（膨胀层）前面

• 由于在rang_sensor_layer源码中clear_on_max_reading(清理最大的超声波层数据)规定
  if (range_message.range == range_message.max_range && clear_on_max_reading_)
  clear_sensor_cone = true;
  所以在发布超声波话题时，需要将大于max_rang的数据赋值为max_rang。

• 待完善