【 聲明：版權(quán)所有，歡迎轉(zhuǎn)載，請勿用于商業(yè)用途。 聯(lián)系信箱：feixiaoxing @163.com】

? ? ? ? 3d 點(diǎn)云設(shè)備現(xiàn)在汽車上用的很多。之前3d lidar這種高端傳感器，只能被少部分智能汽車使用。后來很多國產(chǎn)廠家也開始研發(fā)3d lidar之后，它的價格快速下跌下來，部分3d lidar的價格已經(jīng)降到了幾千元左右，實(shí)用性一下子就提升上來了。不管用它來做slam，還是用來檢測物體、識別物體、避障檢測，都是很方便的。所以，對于slam的同學(xué)來說，除了輪速編碼器、imu、camera、單線lidar這些傳統(tǒng)傳感器之外，對多線lidar、深度攝像機(jī)一定要多加關(guān)注，它們肯定是未來發(fā)展的方向。

? ? ? ? 和圖像主要采用opencv庫一樣，目前3d lidar數(shù)據(jù)主要采用的庫是pcl。

1、編寫pc_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>void PointcloudCB(const sensor_msgs::PointCloud2ConstPtr &msg)
{pcl::PointCloud<pcl::PointXYZ> pointCloudIn;pcl::fromROSMsg(*msg, pointCloudIn);int cloudSize = pointCloudIn.points.size();for(int i = 0; i < cloudSize; i++){ROS_INFO("[i=%d] (%.2f, %.2f, %.2f)",i,pointCloudIn.points[i].x,pointCloudIn.points[i].y,pointCloudIn.points[i].z);}
}int main(int argc, char* argv[])
{ros::init(argc, argv, "pc_node");ROS_WARN("pc_node start");ros::NodeHandle nh;ros::Subscriber pc_sub = nh.subscribe("/kinect2/sd/points", 1, PointcloudCB);ros::spin();return 0;
}

? ? ? ? 代碼不復(fù)雜。首先我們創(chuàng)建一個pc_sub訂閱器，它訂閱了話題/kinect2/sd/points，并且為這個話題準(zhǔn)備了回調(diào)函數(shù)PointcloudCB。在這個回調(diào)函數(shù)里面，代碼對收到的點(diǎn)云數(shù)據(jù)進(jìn)行了打印，分別顯示它們的x/y/z浮點(diǎn)數(shù)值。一般來說，點(diǎn)云還會有一個反光強(qiáng)度的值，但這里沒有提及。

? ? ? ? 多線激光雷達(dá)和單線激光雷達(dá)很相似，只不過多了一個z方向的數(shù)值。也正是因?yàn)檫@個z數(shù)值，讓我們知道了周圍環(huán)境的深度信息，這也是它最有價值的地方。

2、準(zhǔn)備CMakeLists.txt

? ? ? ? 因?yàn)閜c_node.cpp依賴于pcl庫，所以這里有兩件事情要解決。第一件事情，查找一下當(dāng)前的依賴庫里面有沒有pcl，

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS message_generation roscpp rospy std_msgs genmsg tf cv_bridge pcl_ros)
find_package(PCL REQUIRED)

? ? ? ? 第二件事情，就是添加編譯規(guī)則，告訴CMakeLists.txt，pc_node.cpp應(yīng)該怎么編譯，

add_executable(pc_node src/pc_node.cpp)
target_link_libraries(pc_node ${catkin_LIBRARIES})
add_dependencies(pc_node beginner_tutorials_generate_messages_cpp)

3、catkin_make編譯

? ? ? ? pc_node.cpp和CMakeLists.txt都準(zhǔn)備好了，那么就可以開始編譯了。編譯的方法，就是在catkin_ws目錄下面直接輸入catkin_make即可。

4、測試和驗(yàn)證

? ? ? ? 測試的方法其實(shí)和camera是一樣的。第一步，需要一個仿真環(huán)境，輸入roslaunch wpr_simulation wpb_pointcloud.launch即可。

? ? ? ? 仿真環(huán)境準(zhǔn)備好之后，第二步就可以輸入rosrun beginner_tutorials pc_node，這個時候會看到很多的數(shù)據(jù)打印。這些數(shù)據(jù)就是看到的3d數(shù)據(jù)。

[ INFO] [1696920724.029184374, 77.884000000]: [i=16505] (-0.82, -1.09, 2.22)
[ INFO] [1696920724.029235237, 77.884000000]: [i=16506] (-0.81, -1.09, 2.22)
[ INFO] [1696920724.029306573, 77.884000000]: [i=16507] (-0.81, -1.09, 2.22)
[ INFO] [1696920724.029391593, 77.884000000]: [i=16508] (-0.80, -1.09, 2.22)
[ INFO] [1696920724.029448090, 77.884000000]: [i=16509] (-0.79, -1.09, 2.22)
[ INFO] [1696920724.029506356, 77.884000000]: [i=16510] (-0.79, -1.10, 2.24)
[ INFO] [1696920724.029556718, 77.884000000]: [i=16511] (-0.79, -1.11, 2.25)
[ INFO] [1696920724.029606507, 77.884000000]: [i=16512] (-0.79, -1.12, 2.27)
[ INFO] [1696920724.029656374, 77.884000000]: [i=16513] (-0.79, -1.12, 2.29)
[ INFO] [1696920724.029706286, 77.884000000]: [i=16514] (-0.79, -1.13, 2.31)
[ INFO] [1696920724.029756426, 77.884000000]: [i=16515] (-0.79, -1.14, 2.33)
[ INFO] [1696920724.029805764, 77.884000000]: [i=16516] (-0.79, -1.15, 2.35)
[ INFO] [1696920724.029855296, 77.884000000]: [i=16517] (-0.79, -1.16, 2.37)
[ INFO] [1696920724.029904895, 77.884000000]: [i=16518] (-0.79, -1.17, 2.38)
[ INFO] [1696920724.029954521, 77.884000000]: [i=16519] (-0.79, -1.18, 2.40)
[ INFO] [1696920724.030003954, 77.884000000]: [i=16520] (-0.79, -1.19, 2.42)
[ INFO] [1696920724.030053707, 77.884000000]: [i=16521] (-0.79, -1.20, 2.45)
[ INFO] [1696920724.030103470, 77.884000000]: [i=16522] (-0.79, -1.21, 2.47)
[ INFO] [1696920724.030152928, 77.884000000]: [i=16523] (-0.79, -1.22, 2.49)
[ INFO] [1696920724.030202438, 77.884000000]: [i=16524] (-0.79, -1.23, 2.51)
[ INFO] [1696920724.030251816, 77.884000000]: [i=16525] (-0.79, -1.24, 2.53)

5、后續(xù)的工作

? ? ? ? 拿到點(diǎn)云數(shù)據(jù)只是第一步，后續(xù)可以通過x/y/z限制、濾波、分割、識別、統(tǒng)計(jì)等方法，估算出物體的具體位置。拿到這些位置信息之后，就可以進(jìn)一步通知機(jī)器人去進(jìn)行后續(xù)任務(wù)的處理，這個是之前傳感器無法實(shí)現(xiàn)的效果。

? ? ? ? 當(dāng)然現(xiàn)在用3d lidar做slam的開源代碼也很多，特別是室外，靠gmapping根本是不可能的。只能靠3d lidar、gps、imu這些傳感器去處理，也是未來發(fā)展一個很重要的方向。