From 49b3fbb7f031ea1ff7dd32e9f82528883b99ddfd Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Tue, 15 Apr 2025 22:39:32 -0700
Subject: [PATCH 1/3] make dynamic obstacle clearing much faster

---
 go2_robot_sdk/config/nav2_params.yaml | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/go2_robot_sdk/config/nav2_params.yaml b/go2_robot_sdk/config/nav2_params.yaml
index 6b53fde7..eabef61b 100644
--- a/go2_robot_sdk/config/nav2_params.yaml
+++ b/go2_robot_sdk/config/nav2_params.yaml
@@ -181,12 +181,13 @@ controller_server:
 local_costmap:
   local_costmap:
     ros__parameters:
-      update_frequency: 1.0
-      publish_frequency: 1.0
+      update_frequency: 2.0
+      publish_frequency: 2.0
       global_frame: odom
       robot_base_frame: base_link
       use_sim_time: False
       rolling_window: true
+      always_send_full_costmap: true
       width: 6
       height: 6
       resolution: 0.05
@@ -202,15 +203,17 @@ local_costmap:
         enabled: True
         publish_voxel_map: True
         observation_sources: scan
+        decay_time: 0.0
+        combination_method: 0
         scan:
           topic: /scan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
           data_type: "LaserScan"
-          raytrace_max_range: 3.0
+          raytrace_max_range: 6.0
           raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
+          obstacle_max_range: 5.5
           obstacle_min_range: 0.0
 
       inflation_layer:

From 6393cac862c5bf1726b1390dd37357c1993cc81c Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Sat, 26 Apr 2025 22:08:40 -0700
Subject: [PATCH 2/3] removed static layer for local costmap

---
 go2_robot_sdk/config/nav2_params.yaml | 19 +++++++------------
 1 file changed, 7 insertions(+), 12 deletions(-)

diff --git a/go2_robot_sdk/config/nav2_params.yaml b/go2_robot_sdk/config/nav2_params.yaml
index eabef61b..9fa2d882 100644
--- a/go2_robot_sdk/config/nav2_params.yaml
+++ b/go2_robot_sdk/config/nav2_params.yaml
@@ -188,23 +188,16 @@ local_costmap:
       use_sim_time: False
       rolling_window: true
       always_send_full_costmap: true
-      width: 6
-      height: 6
+      width: 8
+      height: 8
       resolution: 0.05
       footprint: "[ [0.360, 0.200], [0.360, -0.200], [-0.450, -0.200], [-0.450, 0.200] ]"
-      plugins: ["static_layer", "voxel_layer", "inflation_layer"]
-
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
+      plugins: ["obstacle_layer", "inflation_layer"]
 
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
-        publish_voxel_map: True
         observation_sources: scan
-        decay_time: 0.0
-        combination_method: 0
         scan:
           topic: /scan
           max_obstacle_height: 2.0
@@ -248,6 +241,8 @@ global_costmap:
         enabled: True
         publish_voxel_map: True
         observation_sources: scan
+        decay_time: 0.0
+        combination_method: 0
         scan:
           topic: scan
           max_obstacle_height: 2.0

From 2220e6886552b1e8fc48428926b4d36cbed59e6c Mon Sep 17 00:00:00 2001
From: Stash Pomichter <pomichterstash@gmail.com>
Date: Tue, 29 Apr 2025 17:31:32 -0700
Subject: [PATCH 3/3] Removed continuous updating voxel costmap layer from
 global map creation

---
 go2_robot_sdk/config/nav2_params.yaml | 22 +++-------------------
 1 file changed, 3 insertions(+), 19 deletions(-)

diff --git a/go2_robot_sdk/config/nav2_params.yaml b/go2_robot_sdk/config/nav2_params.yaml
index 9fa2d882..deef1e3f 100644
--- a/go2_robot_sdk/config/nav2_params.yaml
+++ b/go2_robot_sdk/config/nav2_params.yaml
@@ -217,7 +217,7 @@ local_costmap:
 global_costmap:
   global_costmap:
     ros__parameters:
-      update_frequency: 1.0
+      update_frequency: 0.0  # Set to 0 to prevent dynamic updates
       publish_frequency: 1.0
       global_frame: odom
       robot_base_frame: base_link
@@ -230,29 +230,13 @@ global_costmap:
       origin_x: -250.0 
       origin_y: -250.0
       footprint: "[ [0.360, 0.200], [0.360, -0.200], [-0.450, -0.200], [-0.450, 0.200] ]"
-      plugins: ["static_layer", "voxel_layer", "inflation_layer"]
+      plugins: ["static_layer", "inflation_layer"] # Removed voxel_layer
 
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
-
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
-        publish_voxel_map: True
-        observation_sources: scan
-        decay_time: 0.0
-        combination_method: 0
-        scan:
-          topic: scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
+        subscribe_to_updates: False # Don't subscribe to map updates
 
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"