从零搭建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} SHARED
    src/livox_ros_driver2.cpp
    src/lddc.cpp
    src/driver_node.cpp
    src/lds.cpp
    src/lds_lidar.cpp
    src/comm/comm.cpp
    src/comm/ldq.cpp
    src/comm/semaphore.cpp
    src/comm/lidar_imu_data_queue.cpp
    src/comm/cache_index.cpp
    src/comm/pub_handler.cpp
    src/parse_cfg_file/parse_cfg_file.cpp
    src/parse_cfg_file/parse_livox_lidar_cfg.cpp
    src/call_back/lidar_common_callback.cpp
    src/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}_node
  src/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_ros


void 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，接收数据即可.