32线镭神雷达跑LeGO-LOAM：3D 激光SLAM

• 序

最近老板给了个镭神的32线雷达让我搞一下3D SLAM，决定跑一下LeGO-LOAM。但是LeGO-LOAM用的是Velodyne雷达，需要自己修改一些东西。记录一下修改过程。

安装LeGO-LOAM

我的系统：
ubuntu 16.04
ros kinetic

具体的这个过程就不过多赘述了，有很多帖子是教安装的，我参考了以下博客：

https://blog.csdn.net/weixin_39781401/article/details/89526028

https://blog.csdn.net/learning_tortosie/article/details/86527542

里面讲解的很细，向前辈致敬。
另外，贴上LeGO-LOAM源作者github

https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

镭神雷达的相关修改

首先LeGO-LOAM默认的接受的topic name是velodyne_points，点云的frame_id是velodyne，镭神驱动发布的topic name为lslidar_point_cloud，frame_id为laser_link,这里需要修改一下lslidar_c32.launch文件，remap一下topic name，再修改frame_id为velodyne。这是我搞的最简单的方法，其他写程序转也可以。
贴一下launch文件：

<launch>
  <arg name="device_ip" default="192.168.1.200" />
  <arg name="msop_port" default="2368" />
  <arg name="difop_port" default="2369" />
  <arg name="return_mode" default="1" />
  <arg name="time_synchronization" default="true" />

  <node pkg="lslidar_c32_driver" type="lslidar_c32_driver_node" name="lslidar_c32_driver_node" output="screen">
    <param name="device_ip" value="$(arg device_ip)" />
    <param name="msop_port" value="$(arg msop_port)" />
    <param name="difop_port" value="$(arg difop_port)"/>
    <param name="frame_id" value="velodyne"/>
    <param name="add_multicast" value="false"/>
    <param name="group_ip" value="224.1.1.2"/>
    <param name="rpm" value="600"/>
    <param name="return_mode" value="$(arg return_mode)"/>
    <param name="time_synchronization" value="$(arg time_synchronization)"/>
  </node>

  <node pkg="lslidar_c32_decoder" type="lslidar_c32_decoder_node" name="lslidar_c32_decoder_node" output="screen">
    <param name="min_range" value="0.15"/>
    <param name="max_range" value="150.0"/>
    <param name="return_mode" value="$(arg return_mode)"/>
    <param name="time_synchronization" value="$(arg time_synchronization)"/>
    <remap from="lslidar_point_cloud" to="/velodyne_points" />
  </node>

修改完以后LeGO-LOAM就能接收到雷达的点云数据了。

LeGO-LOAM的修改

这主要需要修改utility.h以及imageProjection.cpp[^2]

看一眼源作者告诉大家需要修改的地方：

New Lidar:The key thing to adapt the code to a new sensor is making sure the point cloud can be properly projected to an range image and ground can be correctly detected. For example, VLP-16 has a angular resolution of 0.2° and 2° along two directions. It has 16 beams. The angle of the bottom beam is -15°. Thus, the parameters in “utility.h” are listed as below. When you implement new sensor, make sure that the ground_cloud has enough points for matching. Before you post any issues, please read this.

新雷达：使代码适应新的传感器的关键是确保点云可以正确地投影到距离图像和地面可以正确地检测。例如，VLP-16沿两个方向的角分辨率分别为0.2和2。它有16根光束。底部横梁的角度是-15。因此，在“实用程序”中的参数。h”列出如下。在实现新传感器时，请确保地面云具有足够的匹配点。在你发布任何问题之前，请阅读这篇文章。

New: a new useCloudRing flag has been added to help with point cloud projection. Velodyne point cloud has “ring” channel that directly gives the point row id in a range image. Other lidars may have a same type of channel, i.e., “r” in Ouster. If you are using a non-Velodyne lidar but it has a similar “ring” channel, you can change the PointXYZIR definition in utility.h and the corresponding code in imageProjection.cpp.

新的：增加了一个新的useCloudRing标志来帮助点云投影。Velodyne点云有“环”通道，直接给点行id在一个范围内的图像。其他lidars可能具有相同类型的通道，即"r"是财产侵占。如果您使用的是非velodyne激光雷达，但是它有一个类似的“环形”通道，您可以在utility.h中更改PointXYZIR定义，并在imageproject .cpp中更改相应的代码。

更多详情到源作者github上看吧

修改utility.h

1. 雷达参数修改

这位前辈已经说的很清楚了，可以移步以下博客
https://www.cnblogs.com/hgl0417/p/11067660.html

// LeiShen-32C-5Hz
// extern const int N_SCAN = 32;
// extern const int Horizon_SCAN = 4000;
// extern const float ang_res_x = 360.0 / float(Horizon_SCAN);
// extern const float ang_res_y = 26 / float(N_SCAN-1);
// extern const float ang_bottom = 16.5;
// extern const int groundScanInd = 10; //地面的线扫条数

// LeiShen-32C-10Hz
extern const int N_SCAN = 32;//32线雷达
extern const int Horizon_SCAN = 2000;//每条线发射的点数
extern const float ang_res_x = 360.0 / float(Horizon_SCAN);//水平分辨率
extern const float ang_res_y = 26.0 / float(N_SCAN-1);//垂直分辨率
extern const float ang_bottom = 16.5;
extern const int groundScanInd = 10; //地面的线扫条数

参数一定要设置对，不然性能差别很大。确定自己的雷达设置的多少Hz，一般是默认10Hz。

2. useCloudRing修改

镭神好像是没有Velodyne雷达的ring channel功能，所以useCloudRing设置为false了。

extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used

修改imageproject.cpp

src文件夹找到imageproject.cpp，因为原作者可能一直在更新代码，所以说改哪一行不科学，行数可能变，所以我还是把修改的代码贴一下：

找到copyPointCloud函数

    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
     

        cloudHeader = laserCloudMsg->header;
        cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
        // Remove Nan points
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
        // have "ring" channel in the cloud
        if (useCloudRing == true){
     
            pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
            if (laserCloudInRing->is_dense == false) {
     
                ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
                ros::shutdown();
            }  
        }
    }

这里是把cloudHeader.stamp = ros::Time::now();的注释去掉了。

Enjoy youself

跑rosbag

这个没有什么好说的，用镭神启动雷达

roslaunch lslidar_c32_decoder lslidar_c32.launch --screen

然后录制rosbag

rosbag record -a

跑rosbag是不需要修改LeGO-LOAM的launch文件的，直接来就是。运行run.launch

roslaunch lego_loam run.launch

再播放rosbag，因为没有用到imu，所以只订阅雷达话题

rosbag play *.bag --clock --topic /velodyne_points

实时在线建图

这个应该是大家最想要的了
实时跑需修改launch文件

<launch>
    
    <!--- Sim Time -->
    <param name="/use_sim_time" value="false" />

    <!--- Run Rviz-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lego_loam)/launch/test.rviz" />

    <!--- TF -->
    <node pkg="tf" type="static_transform_publisher" name="camera_init_to_map"  args="0 0 0 1.570795   0        1.570795 /map    /camera_init 10" />
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 -1.570795 -1.570795 0        /camera /base_link   10" />

    <!--- LeGO-LOAM -->    
    <node pkg="lego_loam" type="imageProjection"    name="imageProjection"    output="screen"/>
    <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
    <node pkg="lego_loam" type="mapOptmization"     name="mapOptmization"     output="screen"/>
    <node pkg="lego_loam" type="transformFusion"    name="transformFusion"    output="screen"/>

</launch>

好吧，其实就是这个，由true改为false.

接下来就是enjoy youself

roslaunch lslidar_c32_decoder lslidar_c32.launch --screen

roslaunch lego_loam run.launch

写博客也是力气活儿，转载请注明出处，谢谢。
注：开源不易，如果对你有益，请支持作者的工作。