目录
安装gtsam(Georgia Tech Smoothing and Mapping library, 4.0.0-alpha2)
跑通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
又报错:
/usr/bin/ld: 找不到 -lBoost::serialization
/usr/bin/ld: 找不到 -lBoost::thread
/usr/bin/ld: 找不到 -lBoost::timer
/usr/bin/ld: 找不到 -lBoost::chrono
百度查了一下,说是该类问题有通用的解决方法,先用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,表示不使用仿真时间
测试效果
测试效果如下(雷达放在原位置没有动)