Skip to content

Commit

Permalink
Merge pull request #75 from pedrotomas27/main
Browse files Browse the repository at this point in the history 
Add support for Velodyne Velarray M1600
  • Loading branch information
@JokerJohn
JokerJohn authored Mar 25, 2024
2 parents 0c40ed9 + 629f90a commit b06281f
Show file tree
Hide file tree
Showing 4 changed files with 241 additions and 5 deletions.
168 changes: 168 additions & 0 deletions LIO-SAM-6AXIS/config/params_m1600.yaml
View file
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
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
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
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

0 comments on commit b06281f

Please sign in to comment.