使用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()函數中添加調用：

（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()方法實作中添加 :

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）：

使用RGBD相機模拟雷射資料，用于move_base中添加新圖層避障功能

二、向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記錄：

costmap

圖層無法加入雷射點雲資料，無法實時避障

在模拟雷射資料時，該障礙物層一般設定在

odom

上，是以在導入雷射資料控制雷射高度時，可能雷射資料在

costmap

圖層之下，導緻無法添加到

costmap

中，是以最小雷射高度需要設定為負值。在真實雷射點下也可能出現類似問題，因雷射雷達資料高度和

costmap

圖層不比對，導緻無法避障。

使用RGBD相機模拟雷射資料，用于move_base中添加新圖層避障功能

繼續閱讀

面試題:vector和map的差別，異同。空間分布，100萬資料存哪個比較合适。一、疊代器差別二、vector三、Map、Set四、vector_map 為什麼比map效率高五、如何選擇六、容器選擇原則七、效率對比

C++ 多線程用條件變量确定線程的執行順序而不是使用 sleep(1)

POJ 1284 Primitive Roots (歐拉函數&原根定理)

CQ V1.0分詞bates(基于雙數組tire樹)—應該是目前最快的中文分詞算法

成員函數初始化清單

2021-08-13c++——類之操作符重載

swmm與lisflood-fp源碼如何一起編譯 CMake指令

Windows下VS開發環境環境安裝工程項目設定關于Debug和Release的提示

一文看懂字元串的加減乘除

SLAM主要步驟

C++ 第十五周報告1--《冒泡法排序》

C++實作簡單順序表

C經典書籍筆記——C陷阱與缺陷②(文法陷阱之優先級)一、錯誤案列二、優先級規律

線性表之順序表的實作

C++判斷素數、求最大公約數代碼判斷一個數是否為素數求兩個數的最大公約數

SequoiaDB巨杉資料庫C++驅動概述