updated changed from latest vlm_ros

alexlin2 · alexlin2 · commit c59dbfd674bf · 2026-01-13T20:50:23.000-08:00
diff --git a/src/base_autonomy/local_planner/config/unitree/unitree_g1.yaml b/src/base_autonomy/local_planner/config/unitree/unitree_g1.yaml
@@ -3,7 +3,7 @@ sensorMountingOffsets:
   ros__parameters:
     sensorOffsetX: 0.0
     sensorOffsetY: 0.0
-    sensorOffsetZ: -0.4
+    sensorOffsetZ: 0.0
 
 # SLAM sensor configuration
 feature_extraction_node:
@@ -23,16 +23,20 @@ imu_preintegration_node:
 
 localPlanner:
   ros__parameters:
+    # obstacle height threshold
+    obstacleHeightThre: 0.1 # 0.1 for flat terrain
+
+    # Sensor offset
+    sensorOffsetX: 0.0
+    sensorOffsetY: 0.0
+  
     # Vehicle dimensions
     vehicleLength: 0.5
     vehicleWidth: 0.5
 
     # Speed limits
-    maxSpeed: 0.875
-    autonomySpeed: 0.875
-
-    # obstacle height threshold
-    obstacleHeightThre: 0.05
+    maxSpeed: 0.75
+    autonomySpeed: 0.75
 
     # Path planning
     pathScale: 0.875
@@ -55,15 +59,15 @@ pathFollower:
     maxAccel: 1.5
     goalYawGain: 2.0
 
-    maxSpeed: 0.875
-    autonomySpeed: 0.875
+    maxSpeed: 0.75
+    autonomySpeed: 0.75
 
     # Slow down threshold
     slowDwnDisThre: 0.875
     omniDirDiffThre: 1.5
     yawRateGain: 1.5
     stopYawRateGain: 1.5
-    maxYawRate: 60.0
+    maxYawRate: 40.0 #slow 40.0, fast 60.0
     dirDiffThre: 0.4
     stopDisThre: 0.4
 
diff --git a/src/base_autonomy/terrain_analysis/launch/terrain_analysis.launch b/src/base_autonomy/terrain_analysis/launch/terrain_analysis.launch
@@ -3,7 +3,7 @@
   <node pkg="terrain_analysis" exec="terrainAnalysis" name="terrainAnalysis" output="screen">
     <param name="scanVoxelSize" value="0.05" />
     <param name="decayTime" value="1.0" />
-    <param name="noDecayDis" value="0.0" /> <!-- 1.5 for flat terrain -->
+    <param name="noDecayDis" value="1.5" />
     <param name="clearingDis" value="8.0" />
     <param name="useSorting" value="true" />
     <param name="quantileZ" value="0.25" />
@@ -13,10 +13,10 @@
     <param name="clearDyObs" value="true" />
     <param name="minDyObsDis" value="0.14" />
     <param name="absDyObsRelZThre" value="0.2" />
-    <param name="minDyObsVFOV" value="-30.0" />
-    <param name="maxDyObsVFOV" value="35.0" />
+    <param name="minDyObsVFOV" value="-55.0" />
+    <param name="maxDyObsVFOV" value="10.0" />
     <param name="minDyObsPointNum" value="1" />
-    <param name="minOutOfFovPointNum" value="10" />
+    <param name="minOutOfFovPointNum" value="20" />
     <param name="obstacleHeightThre" value="0.1" />
     <param name="noDataObstacle" value="false" />
     <param name="noDataBlockSkipNum" value="0" />
diff --git a/src/base_autonomy/terrain_analysis/launch/terrain_analysis.launch.py b/src/base_autonomy/terrain_analysis/launch/terrain_analysis.launch.py
@@ -10,7 +10,7 @@
 
 def generate_launch_description():
     # Get robot config from environment variable or use default
-    robot_config_env = os.environ.get('ROBOT_CONFIG_PATH', 'unitree/unitree_go2')
+    robot_config_env = os.environ.get('ROBOT_CONFIG_PATH', 'unitree/unitree_g1')
 
     # Declare launch arguments
     robot_config_arg = DeclareLaunchArgument(
@@ -32,8 +32,8 @@ def generate_launch_description():
         'clearDyObs': True,
         'minDyObsDis': 0.14,
         'absDyObsRelZThre': 0.2,
-        'minDyObsVFOV': -30.0,
-        'maxDyObsVFOV': 35.0,
+        'minDyObsVFOV': -55.0,
+        'maxDyObsVFOV': 10.0,
         'minDyObsPointNum': 1,
         'minOutOfFovPointNum': 20,
         'obstacleHeightThre': 0.1,
diff --git a/src/slam/arise_slam_mid360/config/livox_mid360.yaml b/src/slam/arise_slam_mid360/config/livox_mid360.yaml
@@ -19,17 +19,28 @@ feature_extraction_node:
         scan_line: 4 #16 64
         sensor: "livox"
         mapping_skip_frame: 1
+        lidar_flip: false
+        blindFront: 0.2
+        blindBack: -0.2
+        blindLeft: 0.3
+        blindRight: -0.3
+        blindDiskLow: -0.05
+        blindDiskHigh: 0.05
+        blindDiskRadius: 0.5
         use_imu_roll_pitch: false
         use_dynamic_mask: false
         use_up_realsense_points: false
         use_down_realsense_points: false
-        min_range: 0.15    #<!-- remove too closed points -->
+        min_range: 0.1    #<!-- remove too closed points -->
         max_range: 130.
         skip_realsense_points: 3
         provide_imu_laser_extrinsic: true
         provide_point_time: 1
         debug_view: false
         
+        imu_acc_x_offset: 0.04
+        imu_acc_y_offset: 0.0
+        imu_acc_z_offset: 0.0
         imu_acc_x_limit: 1.0
         imu_acc_y_limit: 1.0
         imu_acc_z_limit: 1.0
@@ -98,6 +109,9 @@ imu_preintegration_node:
         g_norm: 9.80511    # 0 means you need to provide both imu-camera and laser-camera extrinsic
         use_imu_roll_pitch: false
         lidar_flip: false
-        imu_acc_x_limit: 0.4
-        imu_acc_y_limit: 0.4
-        imu_acc_z_limit: 0.2
+        imu_acc_x_offset: 0.04
+        imu_acc_y_offset: 0.0
+        imu_acc_z_offset: 0.0
+        imu_acc_x_limit: 0.3
+        imu_acc_y_limit: 0.2
+        imu_acc_z_limit: 0.1
diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/FeatureExtraction/featureExtraction.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/FeatureExtraction/featureExtraction.h
@@ -96,6 +96,9 @@ namespace arise_slam {
         SensorType sensor;
         double livox_pitch;
 
+        double imu_acc_x_offset;
+        double imu_acc_y_offset;
+        double imu_acc_z_offset;
         double imu_acc_x_limit;
         double imu_acc_y_limit;
         double imu_acc_z_limit;
diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h
@@ -57,6 +57,9 @@ namespace arise_slam {
         bool lidar_flip;
         SensorType sensor;
 
+        double imu_acc_x_offset;
+        double imu_acc_y_offset;
+        double imu_acc_z_offset;
         double imu_acc_x_limit;
         double imu_acc_y_limit;
         double imu_acc_z_limit;
diff --git a/src/slam/arise_slam_mid360/src/FeatureExtraction/featureExtraction.cpp b/src/slam/arise_slam_mid360/src/FeatureExtraction/featureExtraction.cpp
@@ -183,6 +183,9 @@ namespace arise_slam {
         this->declare_parameter<int>("provide_point_time");
         this->declare_parameter<std::string>("sensor");
         this->declare_parameter<bool>("debug_view", false);
+        this->declare_parameter<double>("imu_acc_x_offset");
+        this->declare_parameter<double>("imu_acc_y_offset");
+        this->declare_parameter<double>("imu_acc_z_offset");
         this->declare_parameter<double>("imu_acc_x_limit");
         this->declare_parameter<double>("imu_acc_y_limit");
         this->declare_parameter<double>("imu_acc_z_limit");
@@ -206,6 +209,9 @@ namespace arise_slam {
         config_.provide_point_time = this->get_parameter("provide_point_time").as_int();
         config_.use_dynamic_mask = this->get_parameter("use_dynamic_mask").as_bool(); 
         config_.debug_view_enabled = get_parameter("debug_view").as_bool();
+        config_.imu_acc_x_offset = get_parameter("imu_acc_x_offset").as_double();
+        config_.imu_acc_y_offset = get_parameter("imu_acc_y_offset").as_double();
+        config_.imu_acc_z_offset = get_parameter("imu_acc_z_offset").as_double();
         config_.imu_acc_x_limit = get_parameter("imu_acc_x_limit").as_double();
         config_.imu_acc_y_limit = get_parameter("imu_acc_y_limit").as_double();
         config_.imu_acc_z_limit = get_parameter("imu_acc_z_limit").as_double();
@@ -1197,6 +1203,10 @@ namespace arise_slam {
             msg_in->angular_velocity.y *= -1.0;
             msg_in->angular_velocity.z *= -1.0;
         }
+
+        msg_in->linear_acceleration.x += config_.imu_acc_x_offset;
+        msg_in->linear_acceleration.y += config_.imu_acc_y_offset;
+        msg_in->linear_acceleration.z += config_.imu_acc_z_offset;
        
         m_buf.lock();
         auto msg = msg_in;
diff --git a/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp b/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp
@@ -127,6 +127,9 @@ namespace arise_slam {
         this->declare_parameter<bool>("use_imu_roll_pitch",true);
         this->declare_parameter<bool>("lidar_flip",false);
         this->declare_parameter<std::string>("sensor");
+        this->declare_parameter<double>("imu_acc_x_offset", 0.0);
+        this->declare_parameter<double>("imu_acc_y_offset", 0.0);
+        this->declare_parameter<double>("imu_acc_z_offset", 0.0);
         this->declare_parameter<double>("imu_acc_x_limit", 1.0);
         this->declare_parameter<double>("imu_acc_y_limit", 1.0);
         this->declare_parameter<double>("imu_acc_z_limit", 1.0);
@@ -140,6 +143,9 @@ namespace arise_slam {
         config_.smooth_factor = this->get_parameter("smooth_factor").as_double();
         config_.use_imu_roll_pitch = this->get_parameter("use_imu_roll_pitch").as_bool();
         config_.lidar_flip = this->get_parameter("lidar_flip").as_bool();
+        config_.imu_acc_x_offset = this->get_parameter("imu_acc_x_offset").as_double();
+        config_.imu_acc_y_offset = this->get_parameter("imu_acc_y_offset").as_double();
+        config_.imu_acc_z_offset = this->get_parameter("imu_acc_z_offset").as_double();
         config_.imu_acc_x_limit = this->get_parameter("imu_acc_x_limit").as_double();
         config_.imu_acc_y_limit = this->get_parameter("imu_acc_y_limit").as_double();
         config_.imu_acc_z_limit = this->get_parameter("imu_acc_z_limit").as_double();
@@ -851,6 +857,10 @@ namespace arise_slam {
           imu_raw->angular_velocity.y *= -1.0;
           imu_raw->angular_velocity.z *= -1.0;
         }
+
+        imu_raw->linear_acceleration.x += config_.imu_acc_x_offset;
+        imu_raw->linear_acceleration.y += config_.imu_acc_y_offset;
+        imu_raw->linear_acceleration.z += config_.imu_acc_z_offset;
      
         std::lock_guard<std::mutex> lock(mBuf);
         
