從零搭建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，接收數據即可.