【3D激光SLAM】Livox-mid-360双雷达配置，修改驱动分别发布imu、customsg、pointclouds话题

Hahalim

已于 2024-03-15 16:27:47 修改

阅读量4.5k

点赞数 22

分类专栏： 3D激光SLAM 文章标签： ubuntu 算法 ip 自动驾驶

于 2024-03-14 20:01:24 首次发布

本文链接：https://blog.csdn.net/Hahalim/article/details/136717592

版权

本文详细描述了如何在MID360_config.json中配置多个雷达IP，解决坐标系混乱问题，并将imu、customsg和pointclouds数据分别发布，以便于后续的点云融合处理。同时介绍了如何在msg_MID360.launch文件中进行相关参数调整以适应不同需求，如设置blind阈值以过滤数据。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前言：在MID360_config.json的 "lidar_configs"处增加两个雷达的ip，直接运行 rviz_MID360.launch会显示两个雷达的点云，但是坐标系是乱的，改launch文件里rpy参数只影响点云的坐标变换，而不能改变imu的坐标变化。为方便做点云融合的后续处理，我们把两个雷达的imu、customsg、pointclouds话题分别发布出来，此代码也适用于一个雷达的imu、customsg、pointclouds话题分别发布。

文章目录

1、MID360_config.json

"lidar_configs"处修改

  "lidar_configs" : [
    {
   
      "ip" : "192.168.123.170",
      "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.123.171",
      "pcl_data_type" : 1,
      "pattern_mode" : 0,
      "extrinsic_parameter" : {
   
        "roll": 0.0,
        "pitch": 0.0,
        "yaw": 0.0,
        "x": 0,
        "y": 0,
        "z": 0
      }
    }
  ]
}

此时直接运行 rviz_MID360.launch会显示两个雷达的点云，但是坐标系是乱的，改launch文件里rpy参数只影响点云的坐标变换，而不能改变imu的坐标变化。为方便做点云融合的后续处理，我们把两个雷达的imu、customsg、pointclouds话题分别发布出来，此代码也适用于一个雷达的imu、customsg、pointclouds话题分别发布。

接线情况：两个雷达ip分别为192.168.123.170、192.168.123.171，通过路由器（或者交换机）与主机192.168.123.5相连。

在这里插入图片描述

2、msg_MID360.launch

	<arg name="multi_topic" default="1"/>

17行加上

	<arg name="blind" default="1"/>

31行加上

	<param name="blind" type="double" value="$(arg blind)"/>

3、lddc.cpp

226行处PublishPointcloud2函数里

    CustomMsg livox_msg;
    InitCustomMsg(livox_msg, pkg, index);
    FillPointsToCustomMsg(livox_msg, pkg);
    PublishCustomPointData(livox_msg, index);

替换成

    int point_size_used=0;
    for (size_t i = 0; i < pkg.points.size(); i++)
    {
   
      auto &point = pkg.points[i];
      if(point.x * point.x + point.y * point.y + point.z * point.z >= blind_)
      {
   
        point_size_used++;
      }
    }
    pkg.points_num = point_size_used;
    
    CustomMsg livox_msg;
    InitCustomMsg(livox_msg, pkg, index);
    FillPointsToCustomMsg(livox_msg, pkg);
    PublishCustomPointData(livox_msg, index);

    PointCloud cloud;
    uint64_t timestamp = 0;
    InitPclMsg(pkg, cloud, timestamp);
    FillPointsToPclMsg(pkg, cloud);
    PublishPclData(index, timestamp, cloud);

351行PublishPointcloud2Data函数里
改为

void Lddc::PublishPointcloud2Data(const uint8_t index, const uint64_t timestamp, PointCloud2& cloud) {
   
  std::string ip_string = IpNumToString(lds_->lidars_[index].handle);
  ip_string = ReplacePeriodByUnderline(ip_string);
  cloud.header.frame_id = "livox_frame_" + ip_string;

403行FillPointsToCustomMsg函数里

void Lddc::FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg) {
   
  uint32_t points_num = pkg.points_num;
  const std::vector<PointXyzlt>& points = pkg.points;
  for (uint32_t i = 0; i < points_num; ++i) {
   
    CustomPoint point;
    point.x = points[i].x;
    point.y = points[i].y;
    point.z = points[i].z;
    point.reflectivity = points[i].intensity;
    point.tag = points[i].tag;
    point.line = points[i].line;
    point.offset_time = static_cast<uint32_t>(points[i].offset_time - pkg.base_time);

    livox_msg.points.push_back(std::move(point));
  }
}

替换成

最低0.47元/天解锁文章