镭神C32测试LEGO-LOAM
原文链接
目录
跑通LEGO-LOAM
安装gtsam(Georgia Tech Smoothing and Mapping library, 4.0.0-alpha2)
安装编译LEGO-LOAM
运行
测试
修改utility.h
修改imageProjection.cpp
转换话题文件
修改LEGO-LOAM配置
测试效果
跑通LEGO-LOAM
安装gtsam(Georgia Tech Smoothing and Mapping library, 4.0.0-alpha2)
-
wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip
-
cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/
-
cd ~/Downloads/gtsam-4.0.0-alpha2/
-
mkdir build &&
cd build
-
cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON ..
-
sudo make install
由于系统是ubuntu20.04中文版，所以把上面的【Downloads】换为【下载】即可，中间会有好多warning，不知道是否影响使用。
安装编译LEGO-LOAM
-
cd ~/catkin_ws/src
-
git
clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
-
cd ..
-
catkin_make -j1
第一次编译代码时，需要在“catkin_make”后面加上“-j1”来生成一些消息类型。以后的编译不需要“-j1”
编译时报错：
- In file included from /home/lpf/catkin_ws/src/LeGO-LOAM/LeGO-LOAM/src/imageProjection.cpp:35:/home/lpf/catkin_ws/src/LeGO-LOAM/LeGO-LOAM/include/utility.h:13:10: fatal error: opencv/cv.h: 没有那个文件或目录
13 | #include <opencv/cv.h>
我记得以前是装过opencv的，应该是版本不同，就去搜了一下，原因是在opencv4中opencv2的cv.h融合进了imgproc.hpp里，所以把utility.h中的#include <opencv/cv.h>改成#include <opencv2/imgproc.hpp>即可。
- /home/lpf/catkin_ws/src/LeGO-LOAM/LeGO-LOAM/include/utility.h:182:1: error: ‘mulscalar’ is not a member of ‘pcl::traits’
将cmakelists.txt中的c++编译版本改为14
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3")
- /usr/include/pcl-1.10/pcl/filters/voxel_grid.h:340:21: error: ‘Index’ is not a member of ‘Eigen’
这个错误是因为 Lego LOAM 中原作者建议使用的 gtsam 版本为 4.0.0-alpha2，gtsam 是自带了一个 eigen 库的，而这个版本的 gtsam 中使用的是较低版本的 Eigen，还没有 Eigen::Index 这个定义。比较简单的做法是将 PCL 中用到的 Eigen::Index 改为 int 即可。不过使用 gtsam 中低版本的 Eigen 可能也会在别的库造成问题，所以建议的做法是不适用 gtsam 带的第三方库，具体方法为按照作者的方法编译 gtsam 时在 cmake 阶段设置使用系统自带的 Eigen，此外 gtsam 编译时会默认进行 SSE 加速，而如 PCL 等其他库大多数情况不会打开，因此如果一起使用的话会造成节点崩溃，因此一般将其关闭，如下所示：
-
mkdir build &&
cd build
-
cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON ..
-
sudo make install
又报错：
百度查了一下，说是该类问题有通用的解决方法，先用locate定位未找到的共享库，然后建立软链接，试了一下不行，然后继续搜，找到了这篇，是通过修改CmakeList.txt文件，调用这些包。
-
find_package(Boost REQUIRED COMPONENTS
-
serialization timer thread chrono)
-
-
# include directories
-
include_directories(
-
include
-
${catkin_INCLUDE_DIRS}
-
${PCL_INCLUDE_DIRS}
-
${OpenCV_INCLUDE_DIRS}
-
${GTSAM_INCLUDE_DIR}
-
${Boost_INCLUDE_DIRS}
-
)
-
# link directories
-
link_directories(
-
include
-
${PCL_LIBRARY_DIRS}
-
${OpenCV_LIBRARY_DIRS}
-
${GTSAM_LIBRARY_DIRS}
-
${Boost_INCLUDE_DIRS}
-
)
到此就编译完成了，如下图
运行
因为 tf2 开始已经不会在 frame_id 前面加 /，所以需要将代码中所有涉及 frame_id 的部分将 / 去掉
roslaunch lego_loam run.launch
Notes: launch文件中 The parameter "/use_sim_time" is set to "true" for simulation, "false" to real robot usage.
rosbag play *.bag --clock --topic /velodyne_points /imu/data
--clock指播放时顺带显示录制文件的时间戳，后面表示播放录制文件中指定话题的消息，参考
但是，我一开始没有加参数，rviz没有显示点云，不知道为什么？这个有影响吗？
根据github提供的代码地址下载数据集，播放same_position.bag文件
测试
换用自己的传感器主要需要修改utility.h以及imageProjection.cpp，并转换topic
修改utility.h
源文件作者的备注：
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.
-
extern
const
int N_SCAN =
16;
-
extern
const
int Horizon_SCAN =
1800;
-
extern
const
float ang_res_x =
0.2;
-
extern
const
float ang_res_y =
2.0;
-
extern
const
float ang_bottom =
15.0;
-
extern
const
int groundScanInd =
7;
Another example for Velodyne HDL-32e range image projection:
-
extern
const
int N_SCAN =
32;
//线数
-
extern
const
int Horizon_SCAN =
1800;
//旋转一周的采样数
-
extern
const
float ang_res_x =
360.0/Horizon_SCAN;
//水平分辨率
-
extern
const
float ang_res_y =
41.333/
float(N_Scan
-1);
//垂直分辨率
-
extern
const
float ang_bottom =
30.666666;
//lidar底部线束的角度偏移
-
extern
const
int groundScanInd =
20;
//lidar判定为地面的线束
针对镭神C32，参考，在utility.h中添加镭神C32的配置
-
// 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;
-
extern
const
int Horizon_SCAN =
2000;
-
extern
const
float ang_res_x =
360.0 /
float(Horizon_SCAN);
-
extern
const
float ang_res_y =
31.0 /
float(N_SCAN
-1);
-
extern
const
float ang_bottom =
16.5;
//雷达底部线束的偏移角度
-
extern
const
int groundScanInd =
10;
//地面的线扫条数
查手册，10 Hz时，Horizon_SCAN = 360/0.18 = 2000
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。其他雷达可能也有这个通道。如果您使用的是非velodyne激光雷达，但是它有一个类似的“环形”通道，您可以在utility.h中更改PointXYZIR定义，并在imageproject .cpp中更改相应的代码。
镭神好像是没有Velodyne雷达的ring channel功能，所以useCloudRing设置为false了，设置false后，应该是会通过分辨率和角度来计算属于哪条线束
-
// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
-
extern
const
bool useCloudRing =
false;
// if true, ang_res_y and ang_bottom are not used
修改imageProjection.cpp
找到copyPointCloud，将cloudHeader.stamp = ros::Time::now()的注释去掉
-
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();
-
}
-
}
-
}
转换话题
LeGO-LOAM默认的接受的topic name是velodyne_points，点云的frame_id是velodyne，镭神驱动发布的topic name为lslidar_point_cloud，frame_id为laser_link。所以，要想LEGO-LOAM可以使用镭神C32，有以下两种方法：
- 修改lslidar_c32.launch文件
- 写中间文件，将话题消息作下转换
考虑普适性与便利性，应该写中间文件进行话题转换
topic_change_node.cpp
-
#include "ros/ros.h"
-
#include "std_msgs/String.h"
-
#include "sensor_msgs/PointCloud2.h"
-
#include <string>
-
-
#include <sstream>
-
-
/**
-
* This tutorial demonstrates simple sending of messages over the ROS system.
-
*/
-
-
static ros::Publisher target_pub;
-
std::string target_frame_id;
-
-
static void source_topic_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
-
{
-
sensor_msgs::PointCloud2 msg = *input;
-
msg.header.frame_id = target_frame_id;
-
target_pub.
publish(msg);
-
}
-
-
-
int main(int argc, char *argv[])
-
{
-
-
ros::
init(argc, argv,
"leishen_to_velodye");
-
-
-
ros::NodeHandle private_nh("~");
-
ros::NodeHandle n;
-
-
-
std::string source_topic;
-
std::string target_topic;
-
-
-
-
if (!private_nh.
getParam(
"source_topic", source_topic))
-
{
-
ROS_ERROR(
"can not get source_topic!");
-
exit(
0);
-
}
-
-
if (!private_nh.
getParam(
"target_topic", target_topic))
-
{
-
ROS_ERROR(
"can not get target_topic!");
-
exit(
0);
-
}
-
-
if (!private_nh.
getParam(
"target_frame_id", target_frame_id))
-
{
-
ROS_ERROR(
"can not get target_frame_id!");
-
exit(
0);
-
}
-
-
target_pub = n.
advertise<sensor_msgs::PointCloud2>(target_topic,
10);
-
ros::Subscriber main_topic_sub = n.
subscribe<sensor_msgs::PointCloud2>(source_topic,
10000, source_topic_callback);
-
-
/**
-
* A count of how many messages we have sent. This is used to create
-
* a unique string for each message.
-
*/
-
ros::
spin();
-
-
return
0;
-
}
topic_change.launch
-
<launch>
-
<arg name=
"target_topic_name"
default=
"velodyne_points" />
-
<arg name=
"target_frame_id_name"
default=
"velodyne" />
-
<arg name=
"source_topic_name"
default=
"lslidar_point_cloud" />
-
-
-
<node pkg=
"topic_change" name=
"topic_change_node" type=
"topic_change_node" output=
"screen">
-
<param name=
"target_topic" value=
"$(arg target_topic_name)" />
-
<param name=
"source_topic" value=
"$(arg source_topic_name)" />
-
<param name=
"target_frame_id" value=
"$(arg target_frame_id_name)" />
-
-
</node>
-
</launch>
单独编译：catkin_make -DCATKIN_WHITELIST_PACKAGES="topic_change"
恢复编译所有包：catkin_make -DCATKIN_WHITELIST_PACKAGES=""
修改LEGO-LOAM配置
由于布置的分布式通信系统，需要把镭神lslidar_c32.launch文件中的启动rviz注释掉
对于实时建图，需要修改LEGO-LOAM中的run.launch
-
<!--- Sim Time -->
-
<param name=
"/use_sim_time" value=
"false" />
将true改为false，表示不使用仿真时间
测试效果
测试效果如下（雷达放在原位置没有动）
