LIO-SAM and gazebo weird results

[Funktiona]

I'm trying to setup an working Gazebo environment with LIO-SAM and a Velodyne 16 in Gazebo. I get weird result when LIO-SAM reads the Pointclouds. I'm pretty sure that I have configured the Pointcloud msgs wrong. Does anyone have a ros2 bag with a Velodyne 16. Or can show me a echo on a Pointcloud topic that is currently working. I think I haven't configured the ring information incorrect.

This is what Rviz and Gazebo is showing This is the cloud_registered data and it looks like all points is displayed with an tilt upwards. I made my robot spin around and this was the result after some time. looks like LIO-SAM register all points belonging to the 16 channel/ring. This is a echo on my Pointcloud topic.

Any tips on where and how to start debugging. Or other helpful information would be appreciated! :)

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[jediofgever]

I have noticed that LIO-SAM isn't friendly to simulated LIDAR/ IMUs.

I had similar issues. A few things to look at are;

Is your IMU producing consistent data? , some IMU plugins in Gazebo output jerky data, like huge sudden acceleration or velocities.

Another thing is that, the point cloud data... I used a simulated ouster-64, and I was seeing something similar.

The timestamp of the points is expected to be seconds since imuPreIntegration converts this value to nanoseconds. I had to write a correction node to make LIO-SAM work.

// Write a Node to correct the ouster lidar data

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>
#include <pcl/segmentation/region_growing_rgb.h>

// Eigen includes
#include <Eigen/StdVector>
#include <eigen3/Eigen/Geometry>

// PCL includes
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

struct InOusterPointXYZIRT
{
  PCL_ADD_POINT4D;
  float intensity;
  uint16_t ring;
  float azimuth;
  float distance;
  uint8_t return_type;
  double time_stamp;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(
  InOusterPointXYZIRT,
  (float, x, x)(float, y, y) (float, z, z) (float, intensity, intensity)(uint16_t, ring, ring) (
    float, azimuth, azimuth)(float, distance, distance) (uint8_t, return_type, return_type) (
    double, time_stamp, time_stamp)
)

struct OutOusterPointXYZIRT
{
  PCL_ADD_POINT4D;
  float intensity;
  uint32_t t;
  uint16_t reflectivity;
  uint8_t ring;
  uint16_t noise;
  uint32_t range;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(
  OutOusterPointXYZIRT,
  (float, x, x)(float, y, y) (float, z, z) (float, intensity, intensity)(uint32_t, t, t) (
    uint16_t,
    reflectivity, reflectivity)(uint8_t, ring, ring) (uint16_t, noise, noise) (
    uint32_t, range,
    range)
)

class OusterCorrectionNode : public rclcpp::Node
{
public:
  OusterCorrectionNode()
  : Node("ouster_correction_node")
  {
    // Create a callback function for when messages are received.
    auto callback =
      [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void
      {
        // Create a container for the data.
        sensor_msgs::msg::PointCloud2::SharedPtr output(new sensor_msgs::msg::PointCloud2);

        // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
        pcl::PointCloud<InOusterPointXYZIRT>::Ptr cloud_in(new pcl::PointCloud<InOusterPointXYZIRT>);
        pcl::fromROSMsg(*msg, *cloud_in);

        // Create a container for the data.
        pcl::PointCloud<OutOusterPointXYZIRT>::Ptr cloud_out(new pcl::PointCloud<OutOusterPointXYZIRT>);

        // Convert the ouster data to the correct format
        for (size_t i = 0; i < cloud_in->points.size(); i++) {
          OutOusterPointXYZIRT point;
          point.x = cloud_in->points[i].x;
          point.y = cloud_in->points[i].y;
          point.z = cloud_in->points[i].z;
          point.intensity = cloud_in->points[i].intensity;
          point.t = cloud_in->points[i].time_stamp;
          point.reflectivity = cloud_in->points[i].intensity * 255;
          point.ring = cloud_in->points[i].ring;
          point.noise = 0;
          point.range = cloud_in->points[i].distance * 1000;
          cloud_out->points.push_back(point);
        }

        cloud_out->width = cloud_in->points.size();
        cloud_out->height = 1;
        cloud_out->is_dense = true;

        // Convert the pcl/PointCloud to sensor_msgs/PointCloud2 and publish it
        pcl::toROSMsg(*cloud_out, *output);
        output->header = msg->header;

        // Publish the data.
        publisher_->publish(*output);

      };

    // Create a subscription.
    subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      "/dobbie/sensing/lidar/top/pointcloud_raw_ex", rclcpp::SensorDataQoS(), callback);

    // Create a publisher.
    publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
      "ouster_points_corrected", rclcpp::SensorDataQoS());

  }

  ~OusterCorrectionNode()
  {
  }

private:
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<OusterCorrectionNode>());
  rclcpp::shutdown();
  return 0;
}

[jediofgever]

You will probably need to modify the above code, to adjust IN/OUT point structure to for your sensor as I see in your case the point_step_size is 22

[mbed92]

I have noticed that LIO-SAM isn't friendly to simulated LIDAR/ IMUs.

I had similar issues. A few things to look at are;

Is your IMU producing consistent data? , some IMU plugins in Gazebo output jerky data, like huge sudden acceleration or velocities.

Another thing is that, the point cloud data... I used a simulated ouster-64, and I was seeing something similar.

The timestamp of the points is expected to be seconds since imuPreIntegration converts this value to nanoseconds. I had to write a correction node to make LIO-SAM work.

I implemented my own lidar plugin in Gazebo based on that one with additional ring field and t (nanoseconds). Your answer put me on a right track that there is a type mismatch between my simulated 3D scans and required by the LIO-SAM. Thanks!