使用RGBD相机模拟激光数据，用于move_base中添加新图层避障功能

参考文章：
ROS:depthimage_to_laserscan
ROS导航-向cost-map中添加超声波障碍图层

一、RGBD模拟激光雷达数据
我使用的是RealSense双目相机，首先使用的是ros自带的功能包depthimage_to_lasersca。
github：https://github.com/ros-perception/depthimage_to_laserscan

为了方便自己写.launch文件：

<launch>

    <include file="$(find realsense2_camera)/launch/rs_camera.launch"/>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">\
     <remap from="image" to="/camera/depth/image_rect_raw"/> 
     <remap from="camera_info" to="/camera/depth/camera_info"/>
     <remap from="scan" to="/realsense_scan"/>
     <param name="range_max" type="double" value="4"/>
</node>
</launch>

相机内参信息通常不需要重新映射，因为camera_info将从与图像相同的名称空间进行订阅。

在/depthimage_to_laserscan-melodic-devel/cfg文件夹下可以设置模拟激光数据参数：

scan_height（1）：用于生成Laserscan的像素行数。 对于每一列，扫描将返回图像中垂直居中的那些像素的最小值。
scan_time（1 / 30.0Hz（0.033s））：两次扫描之间的时间（秒）。 通常为1.0 / frame_rate。
range_min（0.45m）：返回的最小范围（以米为单位）。 小于此范围的范围将输出为-Inf。
range_max（10.0m）：返回的最大范围（以米为单位）。 大于此范围的值将输出为+ Inf。
output_frame_id（camera_depth_frame）：激光扫描的Frame ID。 此值应设置为X向前且Z向上的相应帧。

【转载】添加y方向上的阈值：https://zhuanlan.zhihu.com/p/56559798
（1）在Depth.cfg中添加：

gen.add("ythresh_min", double_t, 0, "Minimum y thresh (in meters).", -0.30, -1.0, 1.0)
gen.add("ythresh_max", double_t, 0, "Maximum y thresh (in meters).", 0.30, -1.0, 1.0)

（2）在DepthImageToLaserScanROS类的reconfigureCb()函数中添加调用：

dtl_.set_y_thresh(config.ythresh_min, config.ythresh_max);

（3）在DepthImageToLaserScan类中添加成员函数和成员变量:

void set_y_thresh(const float ythresh_min, const float ythresh_max);
float ythresh_min_;
float ythresh_max_;

同时在DepthImageToLaserScan.cpp中对set_y_thresh()进行具体实现。

void DepthImageToLaserScan::set_y_thresh(const float ythresh_min, const float ythresh_max){
  ythresh_min_ = ythresh_min;
  ythresh_max_ = ythresh_max;
}

（4）在DepthImageToLaserScan.h中Scan类的convert()方法实现中添加 :

const float constant_y = unit_scaling / cam_model.fy(); // line 181

double y = (v - center_y) * depth * constant_y; //line 205
if(y<ythresh_min_||y>ythresh_max_)
{
	r = std::numeric_limits<float>::quiet_NaN();
	continue;
}

在使用y阈值之前，首先要将scan_height调大，由于realsense分辨率为640*480，因此设置scan_height:320。
直接将参数添加至.launch文件中。

<launch>

    <include file="$(find realsense2_camera)/launch/rs_camera.launch"/>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">\
     <remap from="image" to="/camera/depth/image_rect_raw"/> 
     <remap from="camera_info" to="/camera/depth/camera_info"/>
     <remap from="scan" to="/kinect_scan"/>
     <param name="scan_height" type="int" value="320"/>
     <param name="scan_time" type="double" value="0.033"/>
     <param name="range_min" type="double" value="0.45"/>
     <param name="range_max" type="double" value="8"/>
     <param name="output_frame_id" type="string" value="camera_depth_frame"/>
</node>

</launch>

而且由于最终得到的模拟激光数据用于local_map中，近距离使用深度，直接把y范围设置成最大就行。
最终的到模拟的激光点云数据（2D）：

二、向cost_map中添加障碍图层

1.添加新图层ultrasonic_layer，修改文件costmap_common_params.yaml

#---standard pioneer footprint---
#---(in meters)---
#footprint: [ [0.254, -0.0508], [0.1778, -0.0508], [0.1778, -0.1778], [-0.1905, -0.1778], [-0.254, 0], [-0.1905, 0.1778], [0.1778, 0.1778], [0.1778, 0.0508], [0.254, 0.0508] ]
#footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ]
footprint: [ [-0.6,-0.125], [0.0,-0.125], [0.0,0.125], [-0.6,0.125] ]

transform_tolerance: 0.2
map_type: costmap

obstacle_layer:
 enabled: true
 obstacle_range: 3.0
 raytrace_range: 3.5
 inflation_radius: 0.2
 track_unknown_space: false
 combination_method: 1

 observation_sources: laser_scan_sensor
 laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}

# ++++++++++++++++++++
realsense_layer:
 enabled: true
 max_obstacle_height: 1.5 //如果障碍物高度超过此高度就被忽略
 obstacle_range: 3.0
 raytrace_range: 3.5
 inflation_radius: 0.2
 track_unknown_space: false
 combination_method: 1

 observation_sources: realsense_scan_sensor
 laser_scan_sensor: {data_type: LaserScan, topic: kinect_scan, marking: true, clearing: true}
# ++++++++++++++++++++

inflation_layer:
  enabled:              true
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.5  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true
  map_topic:            "/map"

将新图层的数据类型依然设定为LaserScan，而激光数据的Topic改为模拟的激光数据kinect_scan。

2.接下来将新添加的图层加入到localcostmap里面，修改local_costmap_params.yaml文件。

local_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 10.0
  publish_frequency: 10.0
  static_map: false
  rolling_window: true
  width: 5.5
  height: 5.5
  resolution: 0.2
  transform_tolerance: 0.5
  
  plugins:
   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
# ++++++++++++++++
   - {name: realsense_layer,    type: "costmap_2d::VoxelLayer"} 

---

BUG记录：
1.costmap 图层无法加入激光点云数据，无法实时避障
在模拟激光数据时，该障碍物层一般设置在odom上，所以在导入激光数据控制激光高度时，可能激光数据在costmap图层之下，导致无法添加到costmap中，因此最小激光高度需要设置为负值。在真实激光点下也可能出现类似问题，因激光雷达数据高度和costmap图层不匹配，导致无法避障。