镭神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

又报错：

/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，有以下两种方法：

1. 修改lslidar_c32.launch文件
2. 写中间文件，将话题消息作下转换

考虑普适性与便利性，应该写中间文件进行话题转换

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，表示不使用仿真时间

测试效果

测试效果如下（雷达放在原位置没有动）