diff --git a/config/hesai.yaml b/config/hesai.yaml
new file mode 100644
index 0000000..f9ac431
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/include/pointcloud_preprocess.h b/include/pointcloud_preprocess.h
index 4b25424..89f9855 100644
--- a/include/pointcloud_preprocess.h
+++ b/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_;
diff --git a/launch/mapping_hesai.launch b/launch/mapping_hesai.launch
new file mode 100644
index 0000000..2a90676
--- /dev/null
+++ b/launch/mapping_hesai.launch
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/laser_mapping.cc b/src/laser_mapping.cc
index b047958..192286c 100644
--- a/src/laser_mapping.cc
+++ b/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;
diff --git a/src/pointcloud_preprocess.cc b/src/pointcloud_preprocess.cc
index 77be1bc..9788643 100644
--- a/src/pointcloud_preprocess.cc
+++ b/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 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 is_first(num_scans_,true);
+ std::vector yaw_fp(num_scans_, 0.0); // yaw of first scan point
+ std::vector yaw_last(num_scans_, 0.0); // yaw of last scan point
+ std::vector 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