搭建实验室3d slam 移动小车 4.1jackal小车+镭神32线激光雷达lego-loam建图

sudo apt-get install libboost-all-dev
sudo apt-get install cmake

  
  

  
  
1
2

cd ~
git clone https://bitbucket.org/gtborg/gtsam.git

  
  

  
  
1
2

cd ~/gtsam
mkdir build
cd build
cmake ..
make check   #可选的，运行单元测试
sudo  make install

  
  

  
  
1
2
3
4
5
6

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
cd ..
catkin_make -j1

  
  

  
  
1
2
3
4
5

roslaunch lego_loam run.launch

  
  

  
  
1

<launch>
<span class="token operator">&lt;</span><span class="token operator">!</span>--- Sim Time --<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>param name<span class="token operator">=</span><span class="token string">"/use_sim_time"</span> value<span class="token operator">=</span><span class="token string">"true"</span> /<span class="token operator">&gt;</span>

<span class="token operator">&lt;</span><span class="token operator">!</span>--- Run Rviz--<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"rviz"</span> type<span class="token operator">=</span><span class="token string">"rviz"</span> name<span class="token operator">=</span><span class="token string">"rviz"</span> args<span class="token operator">=</span><span class="token string">"-d <span class="token variable"><span class="token variable">$(</span><span class="token function">find</span> lego_loam<span class="token variable">)</span></span>/launch/test.rviz"</span> /<span class="token operator">&gt;</span>

<span class="token operator">&lt;</span><span class="token operator">!</span>--- TF --<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"tf"</span> type<span class="token operator">=</span><span class="token string">"static_transform_publisher"</span> name<span class="token operator">=</span><span class="token string">"camera_init_to_map"</span>  args<span class="token operator">=</span><span class="token string">"0 0 0 1.570795   0        1.570795 /map    /camera_init 10"</span> /<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"tf"</span> type<span class="token operator">=</span><span class="token string">"static_transform_publisher"</span> name<span class="token operator">=</span><span class="token string">"base_link_to_camera"</span> args<span class="token operator">=</span><span class="token string">"0 0 0 -1.570795 -1.570795 0        /camera /base_link   10"</span> /<span class="token operator">&gt;</span>

<span class="token operator">&lt;</span><span class="token operator">!</span>--- LeGO-LOAM --<span class="token operator">&gt;</span>    
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"lego_loam"</span> type<span class="token operator">=</span><span class="token string">"imageProjection"</span>    name<span class="token operator">=</span><span class="token string">"imageProjection"</span>    output<span class="token operator">=</span><span class="token string">"screen"</span>/<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"lego_loam"</span> type<span class="token operator">=</span><span class="token string">"featureAssociation"</span> name<span class="token operator">=</span><span class="token string">"featureAssociation"</span> output<span class="token operator">=</span><span class="token string">"screen"</span>/<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"lego_loam"</span> type<span class="token operator">=</span><span class="token string">"mapOptmization"</span>     name<span class="token operator">=</span><span class="token string">"mapOptmization"</span>     output<span class="token operator">=</span><span class="token string">"screen"</span>/<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"lego_loam"</span> type<span class="token operator">=</span><span class="token string">"transformFusion"</span>    name<span class="token operator">=</span><span class="token string">"transformFusion"</span>    output<span class="token operator">=</span><span class="token string">"screen"</span>/<span class="token operator">&gt;</span>

</launch>

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19

2.示例数据集下载
lego loam测试数据集下载

3.播放bag文件

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

 
 

 
 
1

注意：虽然 /imu/data 是可选的，但如果提供的话，它可以大大提高估计的准确性。
如果你的 IMU 帧与 Velodyne 帧不对齐，使用 IMU 数据将导致明显漂移。
如果对 ”–clock“ 有疑惑，可以参考：https://answers.ros.org/question/12577/when-should-i-need-clock-parameter-on-rosbag-play/ 。

32线镭神雷达跑LeGO-LOAM

摘自原博客：32线镭神雷达跑LeGO-LOAM：3D 激光SLAM

镭神雷达的相关修改

首先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>
  <!--原雷达的ip地址为 192.168.1.200   修改雷达的 target ip为 192.168.80.2-->
  <arg name="device_ip" default="192.168.80.2" />   
  <arg name="msop_port" default="2368" />
  <arg name="difop_port" default="2369" />
  <arg name="return_mode" default="1" />
  <arg name="degree_mode" value="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=“KaTeX parse error: Expected 'EOF', got '&' at position 105: …oken operator">&̲gt;</span> …(arg msop_port)” />
<param name=“difop_port” value=“KaTeX parse error: Expected 'EOF', got '&' at position 105: …oken operator">&̲gt;</span> …(arg return_mode)”/>
<param name=“degree_mode” value=“KaTeX parse error: Expected 'EOF', got '&' at position 106: …oken operator">&̲gt;</span> …(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=“degree_mode” value=“KaTeX parse error: Expected 'EOF', got '&' at position 106: …oken operator">&̲gt;</span> …(arg return_mode)”/>
<param name=“config_vert” value=“true”/>
<param name=“print_vert” value=“false”/>
<param name=“scan_frame_id” value=“laser_link”/>
<param name=“scan_num” value=“15”/>
<param name=“publish_scan” value=“true”/>
<param name=“time_synchronization” value=“$(arg time_synchronization)”/>
<!–lego loam 点云话题改为 /velodyne_points–>
<remap from=“lslidar_point_cloud” to=“/velodyne_points” />
</node>

<!–node name=“rviz” pkg=“rviz” type=“rviz” args=“-d $(find lslidar_c32_decoder)/launch/lslidar_c32.rviz” output=“screen”/–>

</launch>

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44

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

LeGO-LOAM的修改

这主要需要修改utility.h以及imageProjection.cpp
看一眼源作者告诉大家需要修改的地方：

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

雷达参数修改

这位前辈已经说的很清楚了，可以移步以下博客
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; //地面的线扫条数

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15

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

useCloudRing修改

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

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

 
 

 
 
1

修改imageproject.cpp

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

找到copyPointCloud函数

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

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18

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

实验

录制数据集

启动底盘

roslaunch jackal_base base.launch

 
 

 
 
1

启动相机

roslaunch axis_camera axis.launch

 
 

 
 
1

启动镭神激光雷达

roslaunch lslidar_c32_decoder lslidar_c32_lego_loam.launch

 
 

 
 
1

录制数据包 到 /rosbag_manual 目录下
rosbag record 参考博客 ： ROS 中 rosbag 相关命令总结

cd /home/u16l/catkin_ws/rosbag_manual
rosbag record  -a

 
 

 
 
1
2

离线建图

运行 lego-loam

roslaunch lego_loam run.launch

 
 

 
 
1

播放 指定的rosbag

rosbag play *.bag --clock --topic /velodyne_points /imu_data  /axis_camera/image_raw/compressed

 
 

 
 
1

实时在线建图

修改launch文件，其实就是这个，由true改为false.

<launch>
<span class="token operator">&lt;</span><span class="token operator">!</span>--- Sim Time --<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>param name<span class="token operator">=</span><span class="token string">"/use_sim_time"</span> value<span class="token operator">=</span><span class="token string">"false"</span> /<span class="token operator">&gt;</span>

<span class="token operator">&lt;</span><span class="token operator">!</span>--- Run Rviz--<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"rviz"</span> type<span class="token operator">=</span><span class="token string">"rviz"</span> name<span class="token operator">=</span><span class="token string">"rviz"</span> args<span class="token operator">=</span><span class="token string">"-d <span class="token variable"><span class="token variable">$(</span><span class="token function">find</span> lego_loam<span class="token variable">)</span></span>/launch/test.rviz"</span> /<span class="token operator">&gt;</span>

<span class="token operator">&lt;</span><span class="token operator">!</span>--- TF --<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"tf"</span> type<span class="token operator">=</span><span class="token string">"static_transform_publisher"</span> name<span class="token operator">=</span><span class="token string">"camera_init_to_map"</span>  args<span class="token operator">=</span><span class="token string">"0 0 0 1.570795   0        1.570795 /map    /camera_init 10"</span> /<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"tf"</span> type<span class="token operator">=</span><span class="token string">"static_transform_publisher"</span> name<span class="token operator">=</span><span class="token string">"base_link_to_camera"</span> args<span class="token operator">=</span><span class="token string">"0 0 0 -1.570795 -1.570795 0        /camera /base_link   10"</span> /<span class="token operator">&gt;</span>

<span class="token operator">&lt;</span><span class="token operator">!</span>--- LeGO-LOAM --<span class="token operator">&gt;</span>    
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"lego_loam"</span> type<span class="token operator">=</span><span class="token string">"imageProjection"</span>    name<span class="token operator">=</span><span class="token string">"imageProjection"</span>    output<span class="token operator">=</span><span class="token string">"screen"</span>/<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"lego_loam"</span> type<span class="token operator">=</span><span class="token string">"featureAssociation"</span> name<span class="token operator">=</span><span class="token string">"featureAssociation"</span> output<span class="token operator">=</span><span class="token string">"screen"</span>/<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"lego_loam"</span> type<span class="token operator">=</span><span class="token string">"mapOptmization"</span>     name<span class="token operator">=</span><span class="token string">"mapOptmization"</span>     output<span class="token operator">=</span><span class="token string">"screen"</span>/<span class="token operator">&gt;</span>
<span class="token operator">&lt;</span>node pkg<span class="token operator">=</span><span class="token string">"lego_loam"</span> type<span class="token operator">=</span><span class="token string">"transformFusion"</span>    name<span class="token operator">=</span><span class="token string">"transformFusion"</span>    output<span class="token operator">=</span><span class="token string">"screen"</span>/<span class="token operator">&gt;</span>

</launch>

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20

建图时出现 Failed to find match for field ‘intensity’.建立的模型的也有较大的漂移问题解决

在此感谢 广工大的黄同学与镭神智能的员工，帮忙解决。
通过查阅博客 【PCL】Failed to find match for field ‘intensity’.利用点云强度信息，与同行交流得知，出现这个报错的原因主要是雷达的输出的数据，激光强度信息类型不匹配。

激光雷达的数据输出类型如下所示

通过对比得知，lego loam 需要的点云数据类型为， intensity dtype 为 float32

而镭神智能激光雷达输出的 点云数据 intensity dtype 为 uint8

因此解决思路就是，修改数据类型，通过阅读lslidar_c32/lslidar_c32_decoder/include/rawdata.h知道 ，镭神所用的输出data数据类型为 自定义数据类型 VPoint point，故此，将lslidar_c32/lslidar_c32_decoder/src/rawdata.cc 中的 数据类型修改为 PCL常用的点云处理数据类型 pcl::PointXYZI point 即可 ，修改后的注释了 雷达输出的 ring 和 timestamp的两个话题，有待修复。

代码下载

修改后的镭神32线适配lego loam建图 ROS驱动代码下载

link