Add support for Hesai Pandar LiDAR.

config/hesai.yaml

@@ -0,0 +1,48 @@
+common:
+  lid_topic: "/hesai/pandar"
+  imu_topic: "/imu/data"
+  time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+
+preprocess:
+  lidar_type: 4                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for Hesai LiDAR
+  scan_line: 32
+  blind: 4
+  time_scale: 1e-3             # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效
+
+mapping:
+  acc_cov: 0.1
+  gyr_cov: 0.1
+  b_acc_cov: 0.0001
+  b_gyr_cov: 0.0001
+  fov_degree: 180
+  det_range: 100.0
+  extrinsic_est_en: true      # true: enable the online estimation of IMU-LiDAR extrinsic
+  extrinsic_T: [ 0, 0, 0.28 ]
+  extrinsic_R: [ 1, 0, 0,
+                 0, 1, 0,
+                 0, 0, 1 ]
+
+publish:
+  path_publish_en: false
+  scan_publish_en: true       # false: close all the point cloud output
+  scan_effect_pub_en: true    # true: publish the pointscloud of effect point
+  dense_publish_en: false       # false: low down the points number in a global-frame point clouds scan.
+  scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
+
+path_save_en: true                 # 保存轨迹，用于精度计算和比较
+
+pcd_save:
+  pcd_save_en: true
+  interval: -1                 # how many LiDAR frames saved in each pcd file;
+  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
+
+feature_extract_enable: false
+point_filter_num: 6
+max_iteration: 3
+filter_size_surf: 0.5
+filter_size_map: 0.5
+cube_side_length: 1000
+
+ivox_grid_resolution: 0.5        # default=0.2
+ivox_nearby_type: 18             # 6, 18, 26
+esti_plane_threshold: 0.1        # default=0.1

include/pointcloud_preprocess.h

@@ -53,9 +53,30 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
                                   )
 // clang-format on
 
+namespace hesai_ros {
+struct EIGEN_ALIGN16 Point {
+    PCL_ADD_POINT4D;
+    float intensity;
+    double timestamp;
+    uint16_t ring;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace hesai_ros
+
+// clang-format off
+POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point,
+                                  (float, x, x)
+                                  (float, y, y)
+                                  (float, z, z)
+                                  (float, intensity, intensity)
+                                  (double, timestamp, timestamp)
+                                  (std::uint16_t, ring, ring)
+)
+// clang-format on
+
 namespace faster_lio {
 
-enum class LidarType { AVIA = 1, VELO32, OUST64 };
+enum class LidarType { AVIA = 1, VELO32, OUST64,  HESAIxt32}; //{1, 2, 3, 4}
 
 /**
  * point cloud preprocess
@@ -86,6 +107,7 @@ class PointCloudPreprocess {
     void AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
     void Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
     void VelodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+    void HesaiHandler(const sensor_msgs::PointCloud2::ConstPtr &msg);
 
     PointCloudType cloud_full_, cloud_out_;
 

launch/mapping_hesai.launch

@@ -0,0 +1,21 @@
+<launch>
+  <!-- Launch file for Hesai Pandar LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find faster_lio)/config/hesai.yaml" />
+
+    <param name="feature_extract_enable" type="bool" value="0"/>
+    <param name="point_filter_num_" type="int" value="4"/>
+    <param name="max_iteration" type="int" value="3" />
+    <param name="filter_size_surf" type="double" value="0.5" />
+    <param name="filter_size_map" type="double" value="0.5" />
+    <param name="cube_side_length" type="double" value="1000" />
+    <param name="runtime_pos_log_enable" type="bool" value="0" />
+    <node pkg="faster_lio" type="run_mapping_online" name="laserMapping" output="screen" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find faster_lio)/rviz_cfg/loam_livox.rviz" />
+    </group>
+
+</launch>

src/laser_mapping.cc

@@ -105,6 +105,9 @@ bool LaserMapping::LoadParams(ros::NodeHandle &nh) {
     } else if (lidar_type == 3) {
         preprocess_->SetLidarType(LidarType::OUST64);
         LOG(INFO) << "Using OUST 64 Lidar";
+    } else if (lidar_type == 4) {
+        preprocess_->SetLidarType(LidarType::HESAIxt32);
+        LOG(INFO) << "Using Hesai Pandar 32 Lidar";
     } else {
         LOG(WARNING) << "unknown lidar_type";
         return false;
@@ -199,6 +202,9 @@ bool LaserMapping::LoadParamsFromYAML(const std::string &yaml_file) {
     } else if (lidar_type == 3) {
         preprocess_->SetLidarType(LidarType::OUST64);
         LOG(INFO) << "Using OUST 64 Lidar";
+    } else if (lidar_type == 4) {
+        preprocess_->SetLidarType(LidarType::HESAIxt32);
+        LOG(INFO) << "Using Hesai Pandar 32 Lidar";
     } else {
         LOG(WARNING) << "unknown lidar_type";
         return false;

src/pointcloud_preprocess.cc

@@ -26,6 +26,10 @@ void PointCloudPreprocess::Process(const sensor_msgs::PointCloud2::ConstPtr &msg
             VelodyneHandler(msg);
             break;
 
+        case LidarType::HESAIxt32:
+            HesaiHandler(msg);
+            break;
+
         default:
             LOG(ERROR) << "Error LiDAR Type";
             break;
@@ -187,4 +191,88 @@ void PointCloudPreprocess::VelodyneHandler(const sensor_msgs::PointCloud2::Const
     }
 }
 
+void PointCloudPreprocess::HesaiHandler(const sensor_msgs::PointCloud2::ConstPtr &msg)
+{
+    cloud_out_.clear();
+    cloud_full_.clear();
+
+    pcl::PointCloud<hesai_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.points.size();
+    cloud_out_.reserve(plsize);
+
+    /*** These variables only works when no point timestamps given ***/
+    double omega_l = 3.61;  // scan angular velocity
+    std::vector<bool> is_first(num_scans_,true);
+    std::vector<double> yaw_fp(num_scans_, 0.0);      // yaw of first scan point
+    std::vector<float> yaw_last(num_scans_, 0.0);     // yaw of last scan point
+    std::vector<float> time_last(num_scans_, 0.0);    // last offset time
+    /*****************************************************************/
+
+    if (pl_orig.points[plsize - 1].timestamp > 0) {
+        given_offset_time_ = true;
+    } else {
+        given_offset_time_ = false;
+        double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
+        double yaw_end  = yaw_first;
+        int layer_first = pl_orig.points[0].ring;
+        for (uint i = plsize - 1; i > 0; i--) {
+            if (pl_orig.points[i].ring == layer_first) {
+                yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
+                break;
+            }
+        }
+    }
+
+    double time_head = pl_orig.points[0].timestamp;
+
+    for (int i = 0; i < plsize; i++) {
+        PointType added_pt;
+
+        added_pt.normal_x = 0;
+        added_pt.normal_y = 0;
+        added_pt.normal_z = 0;
+        added_pt.x = pl_orig.points[i].x;
+        added_pt.y = pl_orig.points[i].y;
+        added_pt.z = pl_orig.points[i].z;
+        added_pt.intensity = pl_orig.points[i].intensity;
+        added_pt.curvature = (pl_orig.points[i].timestamp - time_head) * 1000.f;  // curvature unit: ms
+
+        if (!given_offset_time_)
+        {
+            int layer = pl_orig.points[i].ring;
+            double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+            if (is_first[layer]) {
+                yaw_fp[layer]=yaw_angle;
+                is_first[layer]=false;
+                added_pt.curvature = 0.0;
+                yaw_last[layer]=yaw_angle;
+                time_last[layer]=added_pt.curvature;
+                continue;
+            }
+
+            // compute offset time
+            if (yaw_angle <= yaw_fp[layer]) {
+                added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;
+            } else {
+                added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l;
+            }
+
+            if (added_pt.curvature < time_last[layer])  added_pt.curvature+=360.0/omega_l;
+
+            yaw_last[layer] = yaw_angle;
+            time_last[layer]=added_pt.curvature;
+        }
+
+        if (i % point_filter_num_ == 0)
+        {
+            if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > (blind_ * blind_)) {
+                cloud_out_.points.push_back(added_pt);
+            }
+        }
+    }
+
+}
+
 }  // namespace faster_lio