功能 ati github 雷達 ack div color back continue

參考：
http://wiki.ros.org/depthimage_to_laserscan
https://github.com/ros-perception/depthimage_to_laserscan
http://blog.csdn.net/Jasmine_shine/article/details/46530143

1.功能描述
將asus_xtion_pro獲取到的深度圖像depthimage進行外點、無效點剔除和圖像有效區域篩選後得到待處理的深度圖像{u,v};
通過深度圖像{u,v}和相機模型參數cam_modle，可以求出深度圖像每個像素點對應的空間點雲{x,y,z};
將空間點雲{x,y,z}投影到xz平面，同時用ythresh_min<y<ythresh_max進行條件約束。

2.代碼修改
在https://github.com/ros-perception/depthimage_to_laserscan代碼基礎上，做如下修改：
（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_;
同時對set_y_thresh()進行具體實現。
（4）在DepthImageToLaserScan類的convert()方法實現中添加 :
double y = (v - center_y) * depth * constant_y;
if(y<ythresh_min_||y>ythresh_max_)
{
r = std::numeric_limits<float>::quiet_NaN();
continue;
}

3.啟動文件
[dtl.launch]:
<launch>
<include file="$(find openni2_launch)/launch/openni2.launch"/>

<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" args="standalone depthimage_to_lasersacn/DepthImageToLaserScanNodelet /camera/rgb/image_viewer">

<remap from="image" to="/camera/depth_registered/image_raw"/>
<remap from="camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="scan" to="/xtion_scan"/>

<param name="scan_height" value="400"/>
<param name="output_frame_id" value="xtion_scan_frame"/>
<param name="range_min" type="double" value="0.45"/>
<param name="range_max" type="double" value="4"/>
<param name="ythresh_min" type="double" value="-0.60"/>
<param name="ythresh_max" type="double" value="0.40"/>

</node>

<node pkg="tf" type="static_transform_publisher" name="xtion_to_laser" args="0 0 0 0 0 0 1 laser xtion_scan_frame 100" />

</launch>

用RGBD模擬激光雷達數據：depthimage_to_laserscan