From fd0ac1c6d628cff6ad8b83f42b94f8f7e0167f40 Mon Sep 17 00:00:00 2001 From: uavfly Date: Sat, 15 Nov 2025 21:59:40 +0800 Subject: [PATCH] Update. --- config/mid360.yaml | 16 ++++++++-------- src/parameters.cpp | 4 +--- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/config/mid360.yaml b/config/mid360.yaml index 4722249..2aa7391 100644 --- a/config/mid360.yaml +++ b/config/mid360.yaml @@ -10,7 +10,7 @@ filter_size_map: 0.5 # Options: 0.5, 0.3, 0.15, 0.1 cube_side_length: 1000.0 # Option: 1000 runtime_pos_log_enable: false # Option: True - + common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" @@ -32,11 +32,11 @@ imu_time_inte: 0.005 # = 1 / frequency of IMU lidar_time_inte: 0.1 satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units - satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units + satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units acc_norm: 1.0 # 9.810 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration lidar_meas_cov: 0.01 # 0.001; 0.01 - acc_cov_output: 500 - gyr_cov_output: 1000 + acc_cov_output: 500.0 + gyr_cov_output: 1000.0 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 imu_meas_acc_cov: 0.1 #0.1 # 0.1 @@ -44,14 +44,14 @@ gyr_cov_input: 0.01 # for IMU as input model acc_cov_input: 0.1 # for IMU as input model plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane - match_s: 81 + match_s: 81.0 ivox_grid_resolution: 2.0 gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # [0.0, 0.0, -9.787561] # gvins # gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] # avia # [0.011, 0.02329, -0.04412] # mid360 - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1 ] + extrinsic_R: [ 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 ] odometry: publish_odometry_without_downsample: false diff --git a/src/parameters.cpp b/src/parameters.cpp index d69c59c..9dbf3fd 100755 --- a/src/parameters.cpp +++ b/src/parameters.cpp @@ -73,7 +73,6 @@ void readParameters(rclcpp::Node::SharedPtr &nh) nh->declare_parameter("mapping.imu_en",true); nh->declare_parameter("mapping.extrinsic_est_en",true); nh->declare_parameter("mapping.imu_time_inte",0.005); - nh->declare_parameter("mapping.lidar_meas_cov",0.1); nh->declare_parameter("mapping.acc_cov_input",0.1); nh->declare_parameter("mapping.vel_cov",20); nh->declare_parameter("mapping.gyr_cov_input",0.1); @@ -88,7 +87,7 @@ void readParameters(rclcpp::Node::SharedPtr &nh) nh->declare_parameter("preprocess.scan_line", 16); nh->declare_parameter("preprocess.scan_rate", 10); nh->declare_parameter("preprocess.timestamp_unit", 1); - nh->declare_parameter("mapping.match_s", 81); + nh->declare_parameter("mapping.match_s", 81.0); nh->declare_parameter>("mapping.gravity", std::vector()); nh->declare_parameter>("mapping.gravity_init", std::vector()); nh->declare_parameter>("mapping.extrinsic_T", std::vector()); @@ -131,7 +130,6 @@ void readParameters(rclcpp::Node::SharedPtr &nh) nh->get_parameter("mapping.imu_en",imu_en); nh->get_parameter("mapping.extrinsic_est_en",extrinsic_est_en); nh->get_parameter("mapping.imu_time_inte",imu_time_inte); - nh->get_parameter("mapping.lidar_meas_cov",laser_point_cov); nh->get_parameter("mapping.acc_cov_input",acc_cov_input); nh->get_parameter("mapping.vel_cov",vel_cov); nh->get_parameter("mapping.gyr_cov_input",gyr_cov_input);