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

Hahalim

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

阅读量2.7k

点赞数 19

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

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

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

版权

3D激光SLAM 专栏收录该内容

2 篇文章 4 订阅

订阅专栏

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

摘要由CSDN通过智能技术生成

前言：在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));
  }
}

替换成

void Lddc::FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg) {
  const std::vector<PointXyzlt>& points = pkg.points;
  for (uint32_t i = 0; i < pkg.points.size(); ++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);
    if(point.x * point.x + point.y * point.y + point.z * point.z >= blind_)
    {
      livox_msg.points.push_back(std::move(point));
    }
  }
}

421行PublishCustomPointData函数里
改为

void Lddc::PublishCustomPointData(CustomMsg& livox_msg, const uint8_t index) {
  std::string ip_string = IpNumToString(lds_->lidars_[index].handle);
  ip_string = ReplacePeriodByUnderline(ip_string);
  livox_msg.header.frame_id = "livox_frame_" + ip_string;

467行FillPointsToPclMsg函数里

  uint32_t points_num = pkg.points_num;
  const std::vector<PointXyzlt>& points = pkg.points;
  for (uint32_t i = 0; i < points_num; ++i) {
    pcl::PointXYZI point;
    point.x = points[i].x;
    point.y = points[i].y;
    point.z = points[i].z;
    point.intensity = points[i].intensity;

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

替换为

  const std::vector<PointXyzlt>& points = pkg.points;
  for (uint32_t i = 0; i < pkg.points.size(); ++i) {
    pcl::PointXYZI point;
    point.x = points[i].x;
    point.y = points[i].y;
    point.z = points[i].z;
    point.intensity = points[i].intensity;
    if(point.x * point.x + point.y * point.y + point.z * point.z >= blind_)
    {
      pcl_msg.points.push_back(std::move(point));
    }
  }

487行PublishPclData函数里
改为

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

491行

#ifdef BUILDING_ROS1
  PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index);
  if (kOutputToRos == output_type_) {
    publisher_ptr->publish(cloud);
  } else {
    if (bag_ && enable_lidar_bag_) {
      bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud);
    }
  }

替换成

#ifdef BUILDING_ROS1
  PublisherPtr publisher_ptr = Lddc::GetPclPublisher(index);
  if (kOutputToRos == output_type_) {
    publisher_ptr->publish(cloud);
  } else {
    if (bag_ && enable_lidar_bag_) {
      bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud);
    }
  }

508行InitImuMsg函数里

void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp,x) {
  imu_msg.header.frame_id = "livox_frame";

替换为

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

537行处改为

  InitImuMsg(imu_data, imu_msg, timestamp, index);

723行加上GetPointCloud2Publisher函数

PublisherPtr Lddc::GetPointCloud2Publisher(uint8_t index) {
  ros::Publisher **pub = nullptr;
  uint32_t queue_size = kMinEthPacketQueueSize;

  if (use_multi_topic_) {
    pub = &private_pub_[index+2];
    queue_size = queue_size / 8; // queue size is 4 for only one lidar
  } else {
    pub = &global_pub_;
    queue_size = queue_size * 8; // shared queue size is 256, for all lidars
  }

  if (*pub == nullptr) {
    char name_str[48];
    memset(name_str, 0, sizeof(name_str));
    if (use_multi_topic_) {
      std::string ip_string = IpNumToString(lds_->lidars_[index].handle);
      snprintf(name_str, sizeof(name_str), "livox/pointcloud/lidar_%s",
               ReplacePeriodByUnderline(ip_string).c_str());
      DRIVER_INFO(*cur_node_, "Support multi topics.");
    } else {
      DRIVER_INFO(*cur_node_, "Support only one topic.");
      snprintf(name_str, sizeof(name_str), "livox/pointcloud/lidar");
    }

    *pub = new ros::Publisher;
    **pub =
          cur_node_->GetNode().advertise<sensor_msgs::PointCloud2>(name_str, queue_size);
      DRIVER_INFO(*cur_node_,
          "%s publish use PointCloud2 format, set ROS publisher queue size %d",
          name_str, queue_size);
  }

  return *pub;
}

759行加上GetPclPublisher函数

PublisherPtr Lddc::GetPclPublisher(uint8_t index)
{
  ros::Publisher **pub = nullptr;
  uint32_t queue_size = kMinEthPacketQueueSize;

  if (use_multi_topic_) {
    pub = &private_pub_[index+2];
    queue_size = queue_size / 8; // queue size is 4 for only one lidar
  } else {
    pub = &global_pub_;
    queue_size = queue_size * 8; // shared queue size is 256, for all lidars
  }

  if (*pub == nullptr) {
    char name_str[48];
    memset(name_str, 0, sizeof(name_str));
    if (use_multi_topic_) {
      std::string ip_string = IpNumToString(lds_->lidars_[index].handle);
      snprintf(name_str, sizeof(name_str), "livox/pcl/lidar_%s",
               ReplacePeriodByUnderline(ip_string).c_str());
      DRIVER_INFO(*cur_node_, "Support multi topics.");
    } else {
      DRIVER_INFO(*cur_node_, "Support only one topic.");
      snprintf(name_str, sizeof(name_str), "livox/pcl/lidar");
    }

    *pub = new ros::Publisher;
    **pub = cur_node_->GetNode().advertise<PointCloud>(name_str, queue_size);
      DRIVER_INFO(*cur_node_,
          "%s publish use pcl PointXYZI format, set ROS publisher queue "
          "size %d",
          name_str, queue_size);
  }

  return *pub;
}

4、lddc.h

95行加上

  void SetBlind(double blind){ blind_ = blind; }

122处改为

  void InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp, const uint8_t index);

112行替换为

  void PublishPointcloud2Data(const uint8_t index, uint64_t timestamp, PointCloud2& cloud);

116行替换为

  void PublishCustomPointData(CustomMsg& livox_msg, const uint8_t index);

120行替换为

  void PublishPclData(const uint8_t index, const uint64_t timestamp, PointCloud& cloud);

134行加上

  PublisherPtr GetPointCloud2Publisher(uint8_t index);
  PublisherPtr GetPclPublisher(uint8_t index);

145行加上

  double blind_{0.0};

5、livox_ros_driver2.cpp

62行加上

  double blind = 0.0;

72行加上

  livox_node.GetNode().getParam("blind", blind);

90行加上

  livox_node.lddc_ptr_->SetBlind(blind);

6、pub_handler.cpp

34行namespace livox_ros里

std::atomic<bool> PubHandler::is_timestamp_sync_;

105行处OnLivoxLidarPointCloudCallback里加上

  if (data->time_type != kTimestampTypeNoSync) {
    is_timestamp_sync_.store(true);
  } else {
    is_timestamp_sync_.store(false);
  }

164行

void PubHandler::CheckTimer() {
  uint8_t lidar_number = lidar_process_handlers_.size();

  for (auto &process_handler : lidar_process_handlers_) {
    uint64_t recent_time_ms = process_handler.second->GetRecentTimeStamp() / kRatioOfMsToNs;
    if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) {
      continue;
    }

    uint64_t diff = process_handler.second->GetRecentTimeStamp() - process_handler.second->GetLidarBaseTime();
    if (diff < publish_interval_tolerance_) {
      continue;
    }

    frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime();
    uint32_t id = process_handler.first;
    points_[id].clear();
    process_handler.second->GetLidarPointClouds(points_[id]);
    if (points_[id].empty()) {
      continue;
    }
    PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num];
    lidar_point.lidar_type = LidarProtoType::kLivoxLidarType;  // TODO:
    lidar_point.handle = id;
    lidar_point.points_num = points_[id].size();
    lidar_point.points = points_[id].data();
    frame_.lidar_num++;
  }

  if (frame_.lidar_num == lidar_number) {
    PublishPointCloud();
    frame_.lidar_num = 0;
  }
  return;
}

替换

void PubHandler::CheckTimer(uint32_t id) {

  if (PubHandler::is_timestamp_sync_.load()) { // Enable time synchronization
    auto& process_handler = lidar_process_handlers_[id];
    uint64_t recent_time_ms = process_handler->GetRecentTimeStamp() / kRatioOfMsToNs;
    if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) {
      return;
    }

    uint64_t diff = process_handler->GetRecentTimeStamp() - process_handler->GetLidarBaseTime();
    if (diff < publish_interval_tolerance_) {
      return;
    }

    frame_.base_time[frame_.lidar_num] = process_handler->GetLidarBaseTime();
    points_[id].clear();
    process_handler->GetLidarPointClouds(points_[id]);
    if (points_[id].empty()) {
      return;
    }
    PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num];
    lidar_point.lidar_type = LidarProtoType::kLivoxLidarType;  // TODO:
    lidar_point.handle = id;
    lidar_point.points_num = points_[id].size();
    lidar_point.points = points_[id].data();
    frame_.lidar_num++;
    
    if (frame_.lidar_num != 0) {
      PublishPointCloud();
      frame_.lidar_num = 0;
    }
  } else { // Disable time synchronization
    auto now_time = std::chrono::high_resolution_clock::now();
    //First Set
    static bool first = true;
    if (first) {
      last_pub_time_ = now_time;
      first = false;
      return;
    }
    if (now_time - last_pub_time_ < std::chrono::nanoseconds(publish_interval_)) {
      return;
    }
    last_pub_time_ += std::chrono::nanoseconds(publish_interval_);
    for (auto &process_handler : lidar_process_handlers_) {
      frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime();
      uint32_t handle = process_handler.first;
      points_[handle].clear();
      process_handler.second->GetLidarPointClouds(points_[handle]);
      if (points_[handle].empty()) {
        continue;
      }
      PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num];
      lidar_point.lidar_type = LidarProtoType::kLivoxLidarType;  // TODO:
      lidar_point.handle = handle;
      lidar_point.points_num = points_[handle].size();
      lidar_point.points = points_[handle].data();
      frame_.lidar_num++;
    }
    PublishPointCloud();
    frame_.lidar_num = 0;
  }
  return;
}

242行增加

      if (raw_packet_queue_.size() < 3) { // reduce CPU usage
        std::this_thread::sleep_for(std::chrono::microseconds(50));
      }

256处改为

    CheckTimer(id);

7、pub_handler.h

103行处改为

  void CheckTimer(uint32_t id);

130行处增加

  static std::atomic<bool> is_timestamp_sync_;

8、编译

source /opt/ros/noetic/setup.sh
./build.sh ROS1

9、运行

$ roslaunch livox_ros_driver2 msg_MID360.launch

$ rostopic list 
/livox/imu_192_168_123_170
/livox/imu_192_168_123_171
/livox/lidar_192_168_123_170
/livox/lidar_192_168_123_171
/livox/pcl/lidar_192_168_123_170
/livox/pcl/lidar_192_168_123_171
/rosout
/rosout_agg

$ rostopic type /livox/lidar_192_168_123_170
livox_ros_driver2/CustomMsg

$ rostopic type /livox/pcl/lidar_192_168_123_170
sensor_msgs/PointCloud2

10、小结

经过不懈努力，我们终于可以得到两个mid360雷达分别发布的imu、customsg、pointclouds话题！
在point_lio建图验证成功！
在这里插入图片描述

在dlo建图验证成功！
在这里插入图片描述

Hahalim

关注

19
点赞
踩
48

收藏

觉得还不错? 一键收藏
9
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫

专栏目录