updated terrain analysis and g1 parameters

alexlin2 · alexlin2 · commit fd8d5500a2e0 · 2025-12-19T23:07:39.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
@@ -11,11 +11,11 @@ feature_extraction_node:
     lidar_flip: false
     blindFront: 0.2
     blindBack: -0.2
-    blindLeft: 0.2
-    blindRight: -0.2
-    blindDiskLow: -0.8
-    blindDiskHigh: 0.8
-    blindDiskRadius: 0.3
+    blindLeft: 0.3
+    blindRight: -0.3
+    blindDiskLow: -0.05
+    blindDiskHigh: 0.05
+    blindDiskRadius: 0.5
 
 imu_preintegration_node:
   ros__parameters:
@@ -28,22 +28,46 @@ localPlanner:
     vehicleWidth: 0.5
 
     # Speed limits
-    maxSpeed: 0.5
+    maxSpeed: 0.875
     autonomySpeed: 0.875
 
+    # obstacle height threshold
+    obstacleHeightThre: 0.05
+
+    # Path planning
+    pathScale: 0.875
+    minPathScale: 0.675
+    pathScaleStep: 0.1
+    minPathRange: 0.8
+    pathRangeStep: 0.6
+    adjacentRange: 3.5
+
     # Goal tolerances
     goalClearRange: 0.6
     goalBehindRange: 0.8
-    goalReachedThreshold: 0.5
+    goalReachedThreshold: 0.3
     goalYawThreshold: 0.15
 
 pathFollower:
   ros__parameters:
     # Control parameters
     lookAheadDis: 0.5
-    maxAccel: 2.0
+    maxAccel: 1.5
     goalYawGain: 2.0
 
+    maxSpeed: 0.875
+    autonomySpeed: 0.875
+
     # Slow down threshold
     slowDwnDisThre: 0.875
-    omniDirDiffThre: 1.5
+    omniDirDiffThre: 1.5
+    yawRateGain: 1.5
+    stopYawRateGain: 1.5
+    maxYawRate: 60.0
+    dirDiffThre: 0.4
+    stopDisThre: 0.4
+
+terrainAnalysis:
+  ros__parameters:
+    minDyObsVFOV: -55.0
+    maxDyObsVFOV: 10.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
@@ -0,0 +1,68 @@
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+
+
+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')
+
+    # Declare launch arguments
+    robot_config_arg = DeclareLaunchArgument(
+        'robot_config',
+        default_value=robot_config_env,
+        description='Robot-specific config file (without .yaml extension)'
+    )
+
+    base_params = {
+        'scanVoxelSize': 0.05,
+        'decayTime': 1.0,
+        'noDecayDis': 1.5,
+        'clearingDis': 8.0,
+        'useSorting': True,
+        'quantileZ': 0.25,
+        'considerDrop': False,
+        'limitGroundLift': False,
+        'maxGroundLift': 0.15,
+        'clearDyObs': True,
+        'minDyObsDis': 0.14,
+        'absDyObsRelZThre': 0.2,
+        'minDyObsVFOV': -30.0,
+        'maxDyObsVFOV': 35.0,
+        'minDyObsPointNum': 1,
+        'minOutOfFovPointNum': 20,
+        'obstacleHeightThre': 0.1,
+        'noDataObstacle': False,
+        'noDataBlockSkipNum': 0,
+        'minBlockPointNum': 10,
+        'vehicleHeight': 1.5,
+        'voxelPointUpdateThre': 100,
+        'voxelTimeUpdateThre': 2.0,
+        'minRelZ': -1.5,
+        'maxRelZ': 0.3,
+        'disRatioZ': 0.2,
+    }
+
+    terrain_node = Node(
+        package='terrain_analysis',   
+        executable='terrainAnalysis',
+        name='terrainAnalysis',
+        output='screen',
+        parameters=[
+            base_params,
+            PythonExpression([
+                "'", FindPackageShare('local_planner'), "/config/",
+                LaunchConfiguration('robot_config'), ".yaml'"
+            ]),
+        ]
+    )
+
+    return LaunchDescription([
+        robot_config_arg,
+        terrain_node,
+    ])
diff --git a/src/base_autonomy/terrain_analysis/src/terrainAnalysis.cpp b/src/base_autonomy/terrain_analysis/src/terrainAnalysis.cpp
@@ -70,8 +70,8 @@ int terrainVoxelHalfWidth = (terrainVoxelWidth - 1) / 2;
 const int terrainVoxelNum = terrainVoxelWidth * terrainVoxelWidth;
 
 // planar voxel parameters
-float planarVoxelSize = 0.1;
-const int planarVoxelWidth = 101;
+float planarVoxelSize = 0.2;
+const int planarVoxelWidth = 51;
 int planarVoxelHalfWidth = (planarVoxelWidth - 1) / 2;
 const int planarVoxelNum = planarVoxelWidth * planarVoxelWidth;
 
@@ -690,4 +690,4 @@ int main(int argc, char **argv) {
   }
 
   return 0;
-}
+}
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch b/src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch
@@ -39,8 +39,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch
@@ -41,8 +41,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch b/src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch
@@ -48,8 +48,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch b/src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch
@@ -39,8 +39,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch
@@ -41,8 +41,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch b/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch
@@ -48,8 +48,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch b/src/base_autonomy/vehicle_simulator/launch/system_simulation.launch
@@ -39,8 +39,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch b/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch
@@ -41,8 +41,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 
diff --git a/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_route_planner.launch b/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_route_planner.launch
@@ -48,8 +48,8 @@ def generate_launch_description():
   )
 
   start_terrain_analysis = IncludeLaunchDescription(
-    FrontendLaunchDescriptionSource(os.path.join(
-      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')
     )
   )
 

Original file line number	Diff line number	Diff line change
`@@ -39,8 +39,8 @@ def generate_launch_description():`
`39`	`39`	`)`
`40`	`40`
`41`	`41`	`start_terrain_analysis = IncludeLaunchDescription(`
`42`		`- FrontendLaunchDescriptionSource(os.path.join(`
`43`		`- get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')`
	`42`	`+ PythonLaunchDescriptionSource(os.path.join(`
	`43`	`+ get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')`
`44`	`44`	`)`
`45`	`45`	`)`
`46`	`46`
Original file line number	Diff line number	Diff line change
`@@ -41,8 +41,8 @@ def generate_launch_description():`
`41`	`41`	`)`
`42`	`42`
`43`	`43`	`start_terrain_analysis = IncludeLaunchDescription(`
`44`		`- FrontendLaunchDescriptionSource(os.path.join(`
`45`		`- get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')`
	`44`	`+ PythonLaunchDescriptionSource(os.path.join(`
	`45`	`+ get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')`
`46`	`46`	`)`
`47`	`47`	`)`
`48`	`48`
Original file line number	Diff line number	Diff line change
`@@ -48,8 +48,8 @@ def generate_launch_description():`
`48`	`48`	`)`
`49`	`49`
`50`	`50`	`start_terrain_analysis = IncludeLaunchDescription(`
`51`		`- FrontendLaunchDescriptionSource(os.path.join(`
`52`		`- get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch')`
	`51`	`+ PythonLaunchDescriptionSource(os.path.join(`
	`52`	`+ get_package_share_directory('terrain_analysis'), 'launch', 'terrain_analysis.launch.py')`
`53`	`53`	`)`
`54`	`54`	`)`
`55`	`55`