devbox
AllinToyou
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：AllinToyou | 2024-05-30 00:56:23

赞

踩

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

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

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29

此时直接运行 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"/>

  • 1

17行加上

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

  • 1

31行加上

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

  • 1

3、lddc.cpp

226行处PublishPointcloud2函数里

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

  • 1
  • 2
  • 3
  • 4

替换成

    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);

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21

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;
  

  • 1
  • 2
  • 3
  • 4
  • 5

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));
  }
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

替换成

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));
    }
  }
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17

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;

  • 1
  • 2
  • 3
  • 4

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));
  }

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11

替换为

  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));
    }
  }

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12

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;

  • 1
  • 2
  • 3
  • 4

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);
    }
  }

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

替换成

#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);
    }
  }

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

508行InitImuMsg函数里

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

  • 1
  • 2

替换为

  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;


  • 1
  • 2
  • 3
  • 4
  • 5

537行处改为

  InitImuMsg(imu_data, imu_msg, timestamp, index);

  • 1

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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35

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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36

4、lddc.h

95行加上

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

  • 1
  • 2

122处改为

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

  • 1

112行替换为

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

  • 1

116行替换为

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

  • 1

120行替换为

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

  • 1

134行加上

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

  • 1
  • 2
  • 3

145行加上

  double blind_{0.0};

  • 1

5、livox_ros_driver2.cpp

62行加上

  double blind = 0.0;

  • 1

72行加上

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

  • 1

90行加上

  livox_node.lddc_ptr_->SetBlind(blind);

  • 1

6、pub_handler.cpp

34行namespace livox_ros里

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

  • 1

105行处OnLivoxLidarPointCloudCallback里加上

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

  • 1
  • 2
  • 3
  • 4
  • 5

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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35

替换

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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64

242行增加

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

  • 1
  • 2
  • 3

256处改为

    CheckTimer(id);

  • 1

7、pub_handler.h

103行处改为

  void CheckTimer(uint32_t id);

  • 1

130行处增加

  static std::atomic<bool> is_timestamp_sync_;

  • 1

8、编译

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

  • 1
  • 2

9、运行

$ roslaunch livox_ros_driver2 msg_MID360.launch 

  • 1
$ 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

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
$ rostopic type /livox/lidar_192_168_123_170
livox_ros_driver2/CustomMsg

  • 1
  • 2
$ rostopic type /livox/pcl/lidar_192_168_123_170
sensor_msgs/PointCloud2

  • 1
  • 2

10、小结

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

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/AllinToyou/article/detail/644656
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号