从零搭建3D激光slam框架-基于mid360雷达节点实现

MID360雷达介绍

雷达SDK编译与测试

雷达驱动的修改、编译与测试

去ros的编译方式

livox_ros_driver2的代码框架介绍

livox_ros_driver2编译

雷达IP配置文件介绍

常见问题介绍

优化改进

MID360雷达介绍

1 硬件介绍：

livox-mid360是大疆的一款非重复扫描固态激光雷达，20万点/秒，可以调整帧率，内部自带6轴IMU，与点云实现硬件同步。售价3999，线缆399，线缆包括网线电源与串口线，网线与雷达通信负责传输雷达点云与IMU数据，电源12V2A，给雷达供电，串口用于与雷达通信，获取雷达工作状态。

2 软件介绍：

雷达软件分为上位机点云测试软件LivoxViewer_2.3.0_Win64，雷达LIVOX-SDK2（https://github.com/Livox-SDK/Livox-SDK2.git），livox-ros_driver2(https://github.com/Livox-SDK/livox_ros_driver2.git)雷达驱动ROS2节点，点云测试软件可以查看点云形态，测试点云质量和通信稳定性。雷达LIVOX-SDK2，负责从雷达通过socket获取网络包，网络包包含点云与imu数据，livox-ros_driver2 负责将livox-sdk2获取的socket网络包进行解析，解析出imu,lidar数据，填充到ros2话题消息中，填充消息有三种格式pclmsg,custommsg,sensor::pointcloud::msg，通常使用第二种自定义消息custommsg。

livox_lidar_quick_start，雷达快速启动程序可以用于测试雷达通信和IP地址读取，实现快速验证雷达功能和软件功能。

雷达SDK编译与测试

Liovx-SDK2编译

编译cmakelists.txt是常规的格式，直接编译即可

mkdir build

cd build

cmake ..

make

编译后生成依赖库 liblivox_sdk_shared.so 用于驱动的依赖，如果需要交叉编译，只需在cmakelists.txt增加交叉编译工具链声明。

同时编译后在sample目录下生成 livox_lidar_quick_start，雷达快速启动程序，用于测试雷达网络IP和数据传输。

雷达驱动的修改、编译与测试

去ros的编译方式

修改cmakelists.txt如下，包含自定义头文件目录和库目录

# judge which cmake codes to use # Copyright(c) 2020 livoxtech limited.cmake_minimum_required(VERSION 3.14)
project(livox_ros_driver2)ADD_COMPILE_OPTIONS(-std=c++17)
set(CMAKE_CXX_FLAGS "-std=c++17 -O3 -g")add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions")
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -pthread -std=c++0x -std=c++17 -fexceptions")
set(CMAKE_POSITION_INDEPENDENT_CODE ON)include_directories(${PROJECT_SOURCE_DIR}/src)
include_directories(${PROJECT_SOURCE_DIR}/3rdparty)include_directories(/usr/include/pcl-1.12)
include_directories(/usr/include/eigen3)
include_directories(/usr/include/opencv4)# livox ros2 driver target
add_library(${PROJECT_NAME} SHAREDsrc/livox_ros_driver2.cppsrc/lddc.cppsrc/driver_node.cppsrc/lds.cppsrc/lds_lidar.cppsrc/comm/comm.cppsrc/comm/ldq.cppsrc/comm/semaphore.cppsrc/comm/lidar_imu_data_queue.cppsrc/comm/cache_index.cppsrc/comm/pub_handler.cppsrc/parse_cfg_file/parse_cfg_file.cppsrc/parse_cfg_file/parse_livox_lidar_cfg.cppsrc/call_back/lidar_common_callback.cppsrc/call_back/livox_lidar_callback.cpp
)target_link_libraries(${PROJECT_NAME}/usr/local/wy_slam_lib/livox2lib/liblivox_lidar_sdk_shared.so  )add_executable(${PROJECT_NAME}_nodesrc/livox_ros_driver2.cpp
)target_link_libraries(${PROJECT_NAME}_node /usr/local/wy_slam_lib/commonlib/libyaml-cpp.so/usr/local/wy_slam_lib/commonlib/libglog.so/usr/local/wy_slam_lib/commonlib/libgflags.so/usr/local/wy_slam_lib/commonlib/libtbb.so/usr/local/wy_slam_lib/commonlib/libtbbmalloc.so/usr/local/wy_slam_lib/commonlib/libtbbmalloc_proxy.so/usr/local/wy_slam_lib/pcllib/libpcl_filters.so/usr/local/wy_slam_lib/pcllib/libpcl_kdtree.so/usr/local/wy_slam_lib/pcllib/libpcl_common.so/usr/local/wy_slam_lib/pcllib/libpcl_registration.so/usr/local/wy_slam_lib/pcllib/libpcl_search.so/usr/local/wy_slam_lib/pcllib/libpcl_io.so/usr/local/wy_slam_lib/pcllib/libpcl_segmentation.so/home/yanyu/Downloads/livox_ros_driver2/src/livox_ros_driver2_no_ros/build/liblivox_ros_driver2.so      )

livox_ros_driver2的代码框架介绍

主节点，driver_node.h/cpp，声明ros2节点，定义lidar/imu数据轮询线程。

lddc.h/cpp负责实际从处理雷达sdk接收的点云与imu数据，通过信号量进行通信，填充lidar/imu数据和时间戳到自定义数据结构或ros2话题消息中。

lds_lidar.h/cpp 调用livox-sdk，并注册雷达点云IMU数据回调函数到sdk，接收socket client_data，并通过信号量通知ROS2节点，处理与发送雷达数据。

livox_ros_driver2编译

编译使用ros格式的编译工具链，支持ros1/ros2，新建 ws_livox_ros_driver2目录，./build.sh humble用于编译ros2格式。

编译后生成liblivox_ros_driver2.so 和雷达节点livox_ros_driver2_node

其中雷达节点livox_ros_driver2_node依赖 liblivox_ros_driver2.so库，是ros2节点，启动后向外发送雷达数据话题和IMU话题 /livox/lidar, /livox/imu，发布频率默认10hz, 200hz，雷达话题带基准时间戳base_time，也是每一帧第一个点的时间戳，以及每个点的时间戳偏移offset_time，格式是uint64, 单位ns，方便进行后续时间戳对齐，以及点云畸变校准。

雷达IP配置文件介绍

MID360_config.json，包含雷达IP，雷达类型，格式等数据定义。雷达IP，可以通过机身二维码下标后两位读取和1组成三位的雷达IP地址，例如后两位为95,则雷达IP为192.168.1.195。也可以通过livox_lidar_quick_start直接获取雷达IP，写入配置文件。

{"lidar_summary_info" : {"lidar_type": 8},"MID360": {"lidar_net_info" : {"cmd_data_port": 56100,"push_msg_port": 56200,"point_data_port": 56300,"imu_data_port": 56400,"log_data_port": 56500},"host_net_info" : {"cmd_data_ip" : "192.168.1.50","cmd_data_port": 56101,"push_msg_ip": "192.168.1.50","push_msg_port": 56201,"point_data_ip": "192.168.1.50","point_data_port": 56301,"imu_data_ip" : "192.168.1.50","imu_data_port": 56401,"log_data_ip" : "","log_data_port": 56501}},"lidar_configs" : [{"ip" : "192.168.1.190","pcl_data_type" : 1,"pattern_mode" : 0,"extrinsic_parameter" : {"roll": 0.0,"pitch": 0.0,"yaw": 0.0,"x": 0,"y": 0,"z": 0}}]
}

配置文件中，雷达IP为192.168.1.190, 网络ip配置为手动192.168.1.50。

常见问题介绍

1 雷达没数据:

检测电源是否接通，电流是否2A以上1A的无法带动，网线是否接通，电脑网卡是否选择正确，配置网络为192.168.1.1,192.168.1.50,255,255,255,0。

用viewer工具查看雷达点云是否正常。

2 雷达数据中断或降频：

用 ros2 topic bw /livox/lidar 查看带宽是否为0.4MB/s，减少其他ROS2话题的订阅，保证流量正常。

3 雷达sdk崩溃

可能与IMU时间戳有关，雷达过热，加装风扇散热。加打印日志用gdb 调试。

优化改进

1 去ros修改，去掉所有的ros 相关的头文件与代码，及编译脚本。

去掉话题发布，用回调函数代替数据传输，增加数据传输稳定性。

2 windows下雷达imu数据获取与建图:

由于viewer可视化软件没有imu数据，以及雷达sdk仅支持linux-ubuntu下的源码开发，因此需要自行修改编译为win-x64格式，去ros后编译为win-x64版本，用回调函数直接获取雷达和IMU数据，实现windows-x64离线建图。

3 部分去ros节点代码修改如下

//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//#include <iostream>
#include <chrono>
#include <vector>
#include <csignal>
#include <thread>#include "include/livox_ros_driver2.h"
#include "include/ros_headers.h"
#include "driver_node.h"
#include "lddc.h"
#include "lds_lidar.h"using namespace livox_ros;namespace livox_ros
{
bool thread_alive_ = true;
DriverNode::DriverNode()
{//DRIVER_INFO(*this, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING);/** Init default system parameter */int xfer_format = kPointCloud2Msg;//kPclPxyziMsg;//kLivoxCustomMsg;//kPointCloud2Msg;//int multi_topic = 0;int data_src = kSourceRawLidar;double publish_freq = 10.0; /* Hz */int output_type = kOutputToRos;std::string frame_id =  "frame_default";if (publish_freq > 100.0) {publish_freq = 100.0;} else if (publish_freq < 0.5) {publish_freq = 0.5;} else {publish_freq = publish_freq;}future_ = exit_signal_.get_future();/** Lidar data distribute control and lidar data source set */lddc_ptr_ = std::make_unique<Lddc>(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id);//lddc_ptr_->SetRosNode(this);if (data_src == kSourceRawLidar) {//DRIVER_INFO(*this, "Data Source is raw lidar.");std::string user_config_path = "../config/MID360_config.json";//this->get_parameter("user_config_path", user_config_path);//DRIVER_INFO(*this, "Config file : %s", user_config_path.c_str());std::string cmdline_bd_code = "000000000000001";//this->get_parameter("cmdline_input_bd_code", cmdline_bd_code);LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq);lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lidar));if ((read_lidar->InitLdsLidar(user_config_path))) {//DRIVER_INFO(*this, "Init lds lidar success!");std::cout<<" Init lds lidar success! "<<std::endl;} else {//DRIVER_ERROR(*this, "Init lds lidar fail!");std::cout<<" Init lds lidar fail! "<<std::endl;}} else {//DRIVER_ERROR(*this, "Invalid data src (%d), please check the launch file", data_src);std::cout<<" Invalid data src, please check the launch file "<<std::endl;}pointclouddata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::PointCloudDataPollThread, this);imudata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::ImuDataPollThread, this);
}}  // namespace livox_rosvoid DriverNode::PointCloudDataPollThread()
{std::future_status status;std::this_thread::sleep_for(std::chrono::seconds(3));do {lddc_ptr_->DistributePointCloudData();status = future_.wait_for(std::chrono::microseconds(0));} while (status == std::future_status::timeout);
}void DriverNode::ImuDataPollThread()
{std::future_status status;std::this_thread::sleep_for(std::chrono::seconds(3));do {lddc_ptr_->DistributeImuData();status = future_.wait_for(std::chrono::microseconds(0));} while (status == std::future_status::timeout);
}void SigHandle(int sig)
{std::cout << "catch sig %d" << sig << std::endl;//rclcpp::shutdown();thread_alive_=false;
}int main(int argc, char** argv)
{//rclcpp::init(argc, argv);signal(SIGINT, SigHandle);//rclcpp::NodeOptions node_options;auto livox_driver_node = std::make_shared<livox_ros::DriverNode>();//rclcpp::spin(livox_driver_node);//if (rclcpp::ok())//    rclcpp::shutdown();while(thread_alive_){usleep(10000);}return 0;
}

增加点云与IMU的回调函数到lddc.h/lddc.cpp，接收数据即可.