ROS下costmap_2d中添加超声波数据辅助导航

---

前言

本文测试了超声波在ros中的使用，因为手中没有超声波，就在仿真环境下进行了测试。仿真环境是基于STDR下进行的，里面已经包含地图、机器人、激光雷达、超声波等常用的仿真环境。
costmap_2d功能包给机器人导航提供了占据栅格地图，接收传感器数据和静态地图信息来更新周围环境信息，ros中的代价地图是使用分层代价地图一起组成我们使用地图，2D场景下代价地图一般由静态地图层(static_layer)、障碍物层(obstacle_layer)、膨胀层(inflation_layer)组成，当然还有其他的层级，根据需求加入，比如超声波层等，每层叠加组成最终的代价地图，现在我们加入sonar_layer层。

---

一、在costmap_2d中添加超声波数据

0.STDR环境准备

直接下载源码，编译即可；配置好move_base等config文件，参考：ros小课堂STDR仿真

0.1超声波数据简介

rosmsg show sensor_msgs/Range
	#传感器类型       0 超声波 1 红外
	uint8 ULTRASOUND=0
	uint8 INFRARED=1
	#数据头
	std_msgs/Header header
	  uint32 seq
	  #时间戳
	  time stamp
	  #坐标系
	  string frame_id
	  #数据类型  0     1 
	uint8 radiation_type
	#检测范围--弧度
	float32 field_of_view
	#测距最小值
	float32 min_range
	#测距最大值
	float32 max_range
	# 实际测距
	float32 range

1.下载range_sensor_layer功能包

下载地址：DLu/navigation_layers
将下载好的功能包放入工作空间中，进行编译。

catkin_make
source ~/.bashrc
rospack plugins --attrib=plugin costmap_2d

输入最后一条指令后，终端会打印range_sensor_layer/costmap_plugins.xml等，说明检测到sensor_layer插件。

2.修改config文件

修改costmap_common_params.yaml文件：
增加超声波层sonar_layer

sonar_layer:
	enabled: true
	#此参数如果为0 ，没有任何影响.否则，如果在这个时间段内没有收到任何声纳数据，超声波层会警告用户，并且此层不被使用
	no_readings_timout: 0.0
	#栅格概率比该值低的标记为free
	clear_threshold: 0.8
	#栅格概率大于该值标定为占据
	mark_threshold: 0.9
	#超声波话题
	topics:["/robot0/sonar_0","/robot0/sonar_1","/robot0/sonar_2","/robot0/sonar_3"]
	#是否将超出sonar最大距离清除
	clear_on_max_reading: true

在local_costmap_params.yaml文件中添加超声波层

plugins:
	- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
	- {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
	- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

如果仅仅作为局部避障，修改这两部分就可以了。
因为超声波层识别到障碍物就会根据超声波扫描角度的范围呈现出一条直线，而不是障碍物的形状，所以在导航时会出现明明是空白区域机器人却无法通过的现象，可以在global_costmap_params.yaml文件中添加超声波层，将超声波信息用于全局规划。

plugins:
	- {name: static_layer, type: "costmap_2d::StaticLayer"}
	- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
	- {name: sonar_layer, type:"range_sensor_layer::RangeSensorLayer"}
	- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

这里还存在一个问题：
在超声波扫描的最远位置处会认为是障碍物，对机器人的导航也会有一定的影响，可以将超声波扫描的角度范围缩小，
修改range_sensor_layer功能包中的range_sensor_layer.cpp

void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::Range& range_message)
{
	if(range_message.range < range_message.min_range || range_message.range > range_message.max_range)
		return ;
	bool clear_sensor_cone = false;
	// change here
	if(range_message.range == range_message.max_range)
	{
		range_message.range = 1024;     //use a large value instead
		if(clear_on_max_reading_)
			clear_sensor_cone = true;
		else
			return; 
	}

	updateCostmap(range_message, clear_sensor)
}

起始位置，正前方超声波没有识别到障碍物，不会增加障碍物地图

运动过程，前后左右均识别到障碍物。可以看到，超声波识别到障碍物会呈现一条直线，覆盖空白区域。

经过以上修改，可以简单的实现添加超声波作为导航过程的传感器，但效果需要自己去调试。

二、将超声波数据转为点云数据Point_Cloud2

1.使用creat_cloud_xyz32转换数据

主要代码如下：

def run(self):
        now = rospy.Time.now()

        sonar0_range = Range()
        sonar0_range.header.stamp = now
        sonar0_range.header.frame_id = "robot0_sonar_0"
        sonar0_range.radiation_type = Range.ULTRASOUND
        sonar0_range.range = self.sonar_range0
        if sonar0_range.range >=self.sonar_max_range or sonar0_range.range <=self.sonar_min_range:
            self.sonar_cloud[0][0] = float("inf")
            self.sonar_cloud[0][1] = float("inf")
        else:
            self.sonar_cloud[0][0] = self.sonar0_pose_x + sonar0_range.range * cos(self.sonar0_pose_yaw)
            self.sonar_cloud[0][1] = self.sonar0_pose_y + sonar0_range.range * sin(self.sonar0_pose_yaw)
	
		#依次处理其余三个超声波数据
		
		# 发布PointCloud2
	    pclcloud = PointCloud2()
        pclcloud.header.frame_id = "/robot0"
        pclcloud = pc2.create_cloud_xyz32(pclcloud.header, self.sonar_cloud)
        self.sonar_pub_cloud.publish(pclcloud)

图中三个绿色的小点即为转换的超声波数据，数据量好少。
个人感觉这种方式处理以后需要用pointcloud_to_laserscan转为激光数据处理会好一点

总结

本文介绍了两种处理超声波数据的方法，用作学习使用。