Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add support for Velodyne Velarray M1600 #75

Merged
merged 1 commit into from
Mar 25, 2024
Merged

Add support for Velodyne Velarray M1600 #75

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
168 changes: 168 additions & 0 deletions LIO-SAM-6AXIS/config/params_m1600.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 ]
6 changes: 4 additions & 2 deletions LIO-SAM-6AXIS/include/utility.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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();
}

Expand Down
18 changes: 18 additions & 0 deletions LIO-SAM-6AXIS/launch/test_m1600.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
<launch>
<arg name="project" default="lio_sam_6axis"/>
<arg name="bag_path" default="/home/pedro/catkin_ws/src/lidar_tests/2_human_walking_illuminated_2023-01-23-001.bag"/>
<!-- Parameters -->
<rosparam file="$(find lio_sam_6axis)/config/params_m1600.yaml" command="load"/>

<!--- LOAM -->
<include file="$(find lio_sam_6axis)/launch/include/module_loam.launch"/>

<!--- Robot State TF -->
<include file="$(find lio_sam_6axis)/launch/include/module_robot_state_publisher.launch"/>

<!--- Run Rviz-->
<node pkg="rviz" type="rviz" name="$(arg project)_rviz"
args="-d $(find lio_sam_6axis)/launch/include/config/vlp.rviz"/>
<node pkg="rosbag" type="play" name="bag_play" args="$(arg bag_path) --clock -d 2 -r 1"/>

</launch>
54 changes: 51 additions & 3 deletions LIO-SAM-6AXIS/src/imageProjection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
Expand Down Expand Up @@ -122,6 +137,7 @@ class ImageProjection : public ParamServer {
pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;
pcl::PointCloud<PandarPointXYZIRT>::Ptr tmpPandarCloudIn;
pcl::PointCloud<Velodyne_M1600PointXYZIRT>::Ptr tmpM1600CloudIn;
pcl::PointCloud<PointType>::Ptr fullCloud;
pcl::PointCloud<PointType>::Ptr extractedCloud;

Expand Down Expand Up @@ -173,6 +189,7 @@ class ImageProjection : public ParamServer {
laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());
tmpPandarCloudIn.reset(new pcl::PointCloud<PandarPointXYZIRT>());
tmpM1600CloudIn.reset(new pcl::PointCloud<Velodyne_M1600PointXYZIRT>());
fullCloud.reset(new pcl::PointCloud<PointType>());
extractedCloud.reset(new pcl::PointCloud<PointType>());

Expand Down Expand Up @@ -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<float>(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());
Expand Down Expand Up @@ -320,7 +357,8 @@ class ImageProjection : public ParamServer {
<< laserCloudIn->points.back().time
<< ", " << laserCloudIn->points.size() << std::endl;
}

vector<int> 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!");
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;

Expand Down