depthimage-to-laserscan

安装

sudo apt-get install ros-melodic-depthimage-to-laserscan
1

depthimage_to_laserscan 功能包核心：
订阅的Topic
image(sensor_msgs/Image)
输入图像信息。
camera_info(sensor_msgs/CameraInfo)
关联图像的相机信息。通常不需要重新映射，因为camera_info将从与image相同的命名空间中进行订阅。
发布的Topic
scan(sensor_msgs/LaserScan)
发布转换成的激光雷达类型数据。
一般需要设置的是:
scan_height(int, default: 1 pixel)
设置用于生成激光雷达信息的象素行数。
scan_time(double, default: 1/30.0Hz (0.033s))
两次扫描的时间间隔。
range_min(double, default: 0.45m)
返回的最小范围。结合range_max使用，只会获取 range_min 与 range_max 之间的数据。
range_max(double, default: 10.0m)
返回的最大范围。结合range_min使用，只会获取 range_min 与 range_max 之间的数据。
output_frame_id(str, default: camera_depth_frame)
激光信息的ID，需要与深度相机的坐标系一致。
################################################################
根据rostopic list中深度相机的深度图像话题修改image和camera_info的内容

为了与2D激光雷达输出的默认激光话题/scan区分，将输出的伪激光话题remap重映射为depth_scan

核心代码

取深度图像上每一列最小值依次保存到雷达ranges[]数组中，这样理论上，我们将会获kinect前方纵向上最近的障碍物距离，ranges[]数组体现了横向每个障碍点到kinect距离。但是实际，纵向上扫面的高度极为苛刻，并不能把地面到到0.6米的高度都扫一遍，因为什么呢，请看后面总结。先看核心代码，思路：先行扫描将数据存到ranges[]中，然后再高度扫面，比较下一行与原来数据ranges[]，小数据替换原来ranges[]中的大数据，这样就将高度给压缩了。

在其中的convert函數中

void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model,
const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height)
計算到圖像中心的偏移量是通過

const int offset = (int)(center_y - scan_height/2);
這裡修改為：

const int offset = 0;

  if (!_sdf->HasElement("pointCloudCutoff"))
    this->point_cloud_cutoff_ = 0.4;
  else
    this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<double>();
  if (!_sdf->HasElement("pointCloudCutoffMax"))
    this->point_cloud_cutoff_max_ = 5.0;
  else

1
2
3
4
5
6
7
8

限制最远5米

<launch>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
        <remap from="image" to="/camera/depth/image_raw" />
        <param name="output_frame_id" value="camera_link"  />
    </node>
</launch>
1
2
3
4
5
6

参考：https://www.itdaan.com/blog/2017/09/14/e707ed6398575abe92857ccb9d01f078.html 核心代码算法讲解
https://blog.csdn.net/YiYeZhiNian/article/details/106084250 代码架构简析