Related adjustments for HESAI configurations

include/utility.h

@@ -58,7 +58,7 @@ using namespace std;
 
 typedef pcl::PointXYZI PointType;
 
-enum class SensorType { VELODYNE, OUSTER, LIVOX };
+enum class SensorType { VELODYNE, OUSTER, LIVOX, HESAI };
 
 class ParamServer
 {
@@ -186,11 +186,15 @@ class ParamServer
         else if (sensorStr == "livox")
         {
             sensor = SensorType::LIVOX;
+        } 
+        else if (sensorStr == "hesai")
+        {
+            sensor = SensorType::HESAI;
         }
         else
         {
             ROS_ERROR_STREAM(
-                "Invalid sensor type (must be either 'velodyne' or 'ouster' or 'livox'): " << sensorStr);
+                "Invalid sensor type (must be either 'velodyne' or 'ouster' or 'livox' or 'hesai'): " << sensorStr);
             ros::shutdown();
         }
 

src/imageProjection.cpp

@@ -30,6 +30,18 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,
     (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)
 )
 
+struct HesaiPointXYZIT {
+    PCL_ADD_POINT4D;
+    float intensity;
+    double timestamp;
+    uint16_t ring;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+POINT_CLOUD_REGISTER_POINT_STRUCT(HesaiPointXYZIT,
+    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
+    (double, timestamp, timestamp) (uint16_t, ring, ring)
+)
+
 // Use the Velodyne point format as a common representation
 using PointXYZIRT = VelodynePointXYZIRT;
 
@@ -68,6 +80,7 @@ class ImageProjection : public ParamServer
 
     pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
     pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;
+    pcl::PointCloud<HesaiPointXYZIT>::Ptr tmpHesaiCloudIn;
     pcl::PointCloud<PointType>::Ptr   fullCloud;
     pcl::PointCloud<PointType>::Ptr   extractedCloud;
 
@@ -108,6 +121,7 @@ class ImageProjection : public ParamServer
     {
         laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
         tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());
+        tmpHesaiCloudIn.reset(new pcl::PointCloud<HesaiPointXYZIT>());
         fullCloud.reset(new pcl::PointCloud<PointType>());
         extractedCloud.reset(new pcl::PointCloud<PointType>());
 
@@ -226,6 +240,31 @@ class ImageProjection : public ParamServer
                 dst.time = src.t * 1e-9f;
             }
         }
+        else if (sensor == SensorType::HESAI)
+        {
+            pcl::moveFromROSMsg(currentCloudMsg, *tmpHesaiCloudIn);
+            laserCloudIn->points.resize(tmpHesaiCloudIn->size());
+            laserCloudIn->is_dense = tmpHesaiCloudIn->is_dense;
+
+            if (tmpHesaiCloudIn->empty()) {
+                ROS_WARN("Received empty Hesai point cloud, skipping frame");
+                return false;
+            }
+
+            double timeBase = tmpHesaiCloudIn->points.front().timestamp;
+            size_t cloudSize = tmpHesaiCloudIn->size();
+            for (size_t i = 0; i < cloudSize; ++i)
+            {
+                auto &src = tmpHesaiCloudIn->points[i];
+                auto &dst = laserCloudIn->points[i];
+                dst.x = src.x;
+                dst.y = src.y;
+                dst.z = src.z;
+                dst.intensity = src.intensity;
+                dst.ring = src.ring;
+                dst.time = static_cast<float>(src.timestamp - timeBase);
+            }
+        }
         else
         {
             ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
@@ -542,7 +581,7 @@ class ImageProjection : public ParamServer
                 continue;
 
             int columnIdn = -1;
-            if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
+            if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER || sensor == SensorType::HESAI)
             {
                 float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
                 static float ang_res_x = 360.0/float(Horizon_SCAN);