diff --git a/include/utility.h b/include/utility.h index cc3c60fd..4e53dcba 100644 --- a/include/utility.h +++ b/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(); } diff --git a/src/imageProjection.cpp b/src/imageProjection.cpp index 49e6abb1..466b6022 100644 --- a/src/imageProjection.cpp +++ b/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::Ptr laserCloudIn; pcl::PointCloud::Ptr tmpOusterCloudIn; + pcl::PointCloud::Ptr tmpHesaiCloudIn; pcl::PointCloud::Ptr fullCloud; pcl::PointCloud::Ptr extractedCloud; @@ -108,6 +121,7 @@ class ImageProjection : public ParamServer { laserCloudIn.reset(new pcl::PointCloud()); tmpOusterCloudIn.reset(new pcl::PointCloud()); + tmpHesaiCloudIn.reset(new pcl::PointCloud()); fullCloud.reset(new pcl::PointCloud()); extractedCloud.reset(new pcl::PointCloud()); @@ -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(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);