From 629f90aa90d65baf67cb1087eb78ebc65f0cbb64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Tom=C3=A1s?= Date: Mon, 25 Mar 2024 16:06:22 +0000 Subject: [PATCH] Add support for Velodyne Velarray M1600 --- LIO-SAM-6AXIS/config/params_m1600.yaml | 168 +++++++++++++++++++++++++ LIO-SAM-6AXIS/include/utility.h | 6 +- LIO-SAM-6AXIS/launch/test_m1600.launch | 18 +++ LIO-SAM-6AXIS/src/imageProjection.cpp | 54 +++++++- 4 files changed, 241 insertions(+), 5 deletions(-) create mode 100644 LIO-SAM-6AXIS/config/params_m1600.yaml create mode 100644 LIO-SAM-6AXIS/launch/test_m1600.launch diff --git a/LIO-SAM-6AXIS/config/params_m1600.yaml b/LIO-SAM-6AXIS/config/params_m1600.yaml new file mode 100644 index 00000000..aec79044 --- /dev/null +++ b/LIO-SAM-6AXIS/config/params_m1600.yaml @@ -0,0 +1,168 @@ +lio_sam_6axis: + + # velodyne16 and stim300(6 axis) + + # Topics + pointCloudTopic: "/m1600/pcl2" # Point cloud data + imuTopic: "/mavros/imu/data" # IMU data + odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU + gpsTopic: "gps_odom" # GPS odometry topic from navsat, see module_navsat.launch file + + # Frames + lidarFrame: "base_link" + baselinkFrame: "base_link" + odometryFrame: "odom" + mapFrame: "map" + + # GPS Settings + useGps: false + useImuHeadingInitialization: false # if using GPS data, set to "true" + useGpsElevation: false # if GPS elevation is bad, set to "false" + gpsCovThreshold: 0 # m^2, threshold for using GPS data + poseCovThreshold: 0 # m^2, threshold for using GPS data + + # debug setting + debugLidarTimestamp: false + debugImu: false + debugGps: false + + # Export settings + savePCD: false # https://github.com/TixiaoShan/LIO-SAM/issues/3 + savePCDDirectory: "/Downloads/LOAM/" # actually we do not use the floder to save maps + + sensor: velodyne_m1600 # lidar sensor type, 'velodyne' or 'ouster' or 'livox' + N_SCAN: 128 # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6) + Horizon_SCAN: 3077 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000) + downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 + lidarMinRange: 0.1 # default: 1.0, minimum lidar range to be used + lidarMaxRange: 100 # default: 1000.0, maximum lidar range to be used + + # IMU Settings + imuFrequence: 190 + imuAccNoise: 0.1 + imuGyrNoise: 0.1 + imuAccBiasN: 0.01 + imuGyrBiasN: 0.01 + imuGravity: 9.80511 + imuRPYWeight: 0.01 + + + + # Extrinsics (lidar -> IMU) + imu_type: 0 # 0: 6axis, 1:9 axis + extrinsicTrans: [-0.011775 -0.006253 -0.0438256454] + extrinsicRot: [1, 0, 0, + 0, 1, 0, + 0, 0, 1] + extrinsicRPY: [1, 0, 0, + 0, 1, 0, + 0, 0, 1] + + # LOAM feature threshold + edgeThreshold: 1.0 + surfThreshold: 0.1 + edgeFeatureMinValidNum: 10 + surfFeatureMinValidNum: 100 + + # voxel filter paprams + odometrySurfLeafSize: 0.2 # default: 0.4 - outdoor, 0.2 - indoor + mappingCornerLeafSize: 0.1 # default: 0.2 - outdoor, 0.1 - indoor + mappingSurfLeafSize: 0.2 # default: 0.4 - outdoor, 0.2 - indoor + + # robot motion constraint (in case you are using a 2D robot) + z_tollerance: 1000 # meters + rotation_tollerance: 1000 # radians + + # CPU Params + numberOfCores: 8 # number of cores for mapping optimization + mappingProcessInterval: 0.15 # seconds, regulate mapping frequency + + # Surrounding map + surroundingkeyframeAddingDistThreshold: 0.2 # meters, regulate keyframe adding threshold + surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold + surroundingKeyframeDensity: 0.5 # meters, downsample surrounding keyframe poses + surroundingKeyframeSearchRadius: 5 # meters, within n meters scan-to-map optimization (when loop closure disabled) + + # Loop closure + loopClosureEnableFlag: false + loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency + surroundingKeyframeSize: 30 # submap size (when loop closure enabled) + historyKeyframeSearchRadius: 30.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure + historyKeyframeSearchTimeDiff: 40.0 # seconds, key frame that is n seconds older will be considered for loop closure + historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure + historyKeyframeFitnessScore: 0.9 # icp threshold, the smaller the better alignment + + # Visualization + globalMapVisualizationSearchRadius: 1000 # meters, global map visualization radius + globalMapVisualizationPoseDensity: 2.0 # meters, global map visualization keyframe density + globalMapVisualizationLeafSize: 0.05 # meters, global map visualization cloud density + + + # mapping + globalMapLeafSize: 0.5 # saved map voxgrid size + + +# Navsat (convert GPS coordinates to Cartesian) +#navsat: +# frequency: 50 +# wait_for_datum: false +# delay: 0.0 +# magnetic_declination_radians: 0 +# yaw_offset: 0 +# zero_altitude: true +# broadcast_utm_transform: false +# broadcast_utm_transform_as_parent_frame: false +# publish_filtered_gps: false +# +## EKF for Navsat +#ekf_gps: +# publish_tf: false +# map_frame: map +# odom_frame: odom +# base_link_frame: base_link +# world_frame: odom +# +# frequency: 50 +# two_d_mode: false +# sensor_timeout: 0.01 +# # ------------------------------------- +# # External IMU: +# # ------------------------------------- +# imu0: imu_correct +# # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link +# imu0_config: [ false, false, false, +# true, true, true, +# false, false, false, +# false, false, true, +# true, true, true ] +# imu0_differential: false +# imu0_queue_size: 50 +# imu0_remove_gravitational_acceleration: true +# # ------------------------------------- +# # Odometry (From Navsat): +# # ------------------------------------- +# odom0: odometry/gps +# odom0_config: [ true, true, true, +# false, false, false, +# false, false, false, +# false, false, false, +# false, false, false ] +# odom0_differential: false +# odom0_queue_size: 10 +# +# # x y z r p y x_dot y_dot z_dot r_dot p_dot y_dot x_ddot y_ddot z_ddot +# process_noise_covariance: [ 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, +# 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, +# 0, 0, 10.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, +# 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, +# 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, +# 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, +# 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, +# 0, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, +# 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, +# 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, +# 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, +# 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, +# 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, +# 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, +# 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015 ] diff --git a/LIO-SAM-6AXIS/include/utility.h b/LIO-SAM-6AXIS/include/utility.h index bc030b6e..f5c41eec 100644 --- a/LIO-SAM-6AXIS/include/utility.h +++ b/LIO-SAM-6AXIS/include/utility.h @@ -60,7 +60,7 @@ using namespace std; typedef pcl::PointXYZI PointType; enum class SensorType { - VELODYNE, OUSTER, LIVOX, HESAI + VELODYNE, OUSTER, LIVOX, HESAI, VELODYNE_M1600 }; class ParamServer { @@ -229,9 +229,11 @@ class ParamServer { sensor = SensorType::LIVOX; } else if (sensorStr == "hesai") { sensor = SensorType::HESAI; + } else if (sensorStr == "velodyne_m1600") { + sensor = SensorType::VELODYNE_M1600; } 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 'velodyne_m1600' ): " << sensorStr); ros::shutdown(); } diff --git a/LIO-SAM-6AXIS/launch/test_m1600.launch b/LIO-SAM-6AXIS/launch/test_m1600.launch new file mode 100644 index 00000000..02474f8d --- /dev/null +++ b/LIO-SAM-6AXIS/launch/test_m1600.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + diff --git a/LIO-SAM-6AXIS/src/imageProjection.cpp b/LIO-SAM-6AXIS/src/imageProjection.cpp index ec18f0b2..e882f6a7 100644 --- a/LIO-SAM-6AXIS/src/imageProjection.cpp +++ b/LIO-SAM-6AXIS/src/imageProjection.cpp @@ -1,5 +1,6 @@ #include "utility.h" #include "lio_sam_6axis/cloud_info.h" +#include "pcl/filters/impl/filter.hpp" struct VelodynePointXYZIRT { PCL_ADD_POINT4D @@ -14,6 +15,20 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) (uint16_t, ring, ring)(float, time, time) ) +struct Velodyne_M1600PointXYZIRT { + PCL_ADD_POINT4D; + uint8_t intensity; + uint8_t ring; + uint32_t timestampSec; + uint32_t timestampNsec; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; +POINT_CLOUD_REGISTER_POINT_STRUCT( + Velodyne_M1600PointXYZIRT, + (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)( + uint8_t, ring, ring)(uint32_t, timestampSec, timestampSec)(uint32_t, timestampNsec, timestampNsec)) + struct PandarPointXYZIRT { PCL_ADD_POINT4D @@ -122,6 +137,7 @@ class ImageProjection : public ParamServer { pcl::PointCloud::Ptr laserCloudIn; pcl::PointCloud::Ptr tmpOusterCloudIn; pcl::PointCloud::Ptr tmpPandarCloudIn; + pcl::PointCloud::Ptr tmpM1600CloudIn; pcl::PointCloud::Ptr fullCloud; pcl::PointCloud::Ptr extractedCloud; @@ -173,6 +189,7 @@ class ImageProjection : public ParamServer { laserCloudIn.reset(new pcl::PointCloud()); tmpOusterCloudIn.reset(new pcl::PointCloud()); tmpPandarCloudIn.reset(new pcl::PointCloud()); + tmpM1600CloudIn.reset(new pcl::PointCloud()); fullCloud.reset(new pcl::PointCloud()); extractedCloud.reset(new pcl::PointCloud()); @@ -285,7 +302,27 @@ class ImageProjection : public ParamServer { dst.time = src.time; // dst.time = (i % 2048) / 20480.0; } - } else if (sensor == SensorType::HESAI) { + } else if (sensor == SensorType::VELODYNE_M1600) { + + pcl::moveFromROSMsg(currentCloudMsg, *tmpM1600CloudIn); + laserCloudIn->points.resize(tmpM1600CloudIn->size()); + laserCloudIn->is_dense = tmpM1600CloudIn->is_dense; + double time_begins = tmpM1600CloudIn->points[0].timestampSec; + double time_beginns = tmpM1600CloudIn->points[0].timestampNsec; + double time_begin = time_begins + (time_beginns * 1e-9); + for (size_t i = 0; i < tmpM1600CloudIn->size(); i++) { + auto &src = tmpM1600CloudIn->points[i]; + auto &dst = laserCloudIn->points[i]; + dst.x = src.x; + dst.y = src.y*-1; + dst.z = src.z*-1; + dst.intensity = static_cast(src.intensity); + dst.ring = src.ring; + double point_time = src.timestampSec + (src.timestampNsec * 1e-9); + dst.time = point_time-time_begin; + } + + }else if (sensor == SensorType::HESAI) { // Convert to Velodyne format pcl::moveFromROSMsg(currentCloudMsg, *tmpPandarCloudIn); laserCloudIn->points.resize(tmpPandarCloudIn->size()); @@ -320,7 +357,8 @@ class ImageProjection : public ParamServer { << laserCloudIn->points.back().time << ", " << laserCloudIn->points.size() << std::endl; } - + vector indices; + pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices); // check dense flag if (laserCloudIn->is_dense == false) { ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!"); @@ -347,7 +385,7 @@ class ImageProjection : public ParamServer { if (deskewFlag == 0) { deskewFlag = -1; for (auto &field : currentCloudMsg.fields) { - if (field.name == "time" || field.name == "t" || field.name == "timestamp") { + if (field.name == "time" || field.name == "t" || field.name == "timestamp" || field.name == "timestampSec") { deskewFlag = 1; break; } @@ -635,8 +673,18 @@ class ImageProjection : public ParamServer { } else if (sensor == SensorType::LIVOX) { columnIdn = columnIdnCountVec[rowIdn]; columnIdnCountVec[rowIdn] += 1; + } else if (sensor == SensorType::VELODYNE_M1600) { + + float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; + + float ang_res_x = 0.29; // or 0.33 for the extended model, as per the specifications + + columnIdn = round((horizonAngle / ang_res_x) + (Horizon_SCAN / 2)); + if (columnIdn >= Horizon_SCAN) + columnIdn -= Horizon_SCAN; } + if (columnIdn < 0 || columnIdn >= Horizon_SCAN) continue;