Tracking error layer

nav2_bringup/params/nav2_params.yaml

@@ -298,7 +298,7 @@ global_costmap:
       robot_radius: 0.22
       resolution: 0.05
       track_unknown_space: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_layer","bounded_tracking_error_layer", "inflation_layer"]
       filters: ["keepout_filter", "speed_filter"]
       keepout_filter:
         plugin: "nav2_costmap_2d::KeepoutFilter"
@@ -331,6 +331,16 @@ global_costmap:
           raytrace_min_range: 0.0
           obstacle_max_range: 2.5
           obstacle_min_range: 0.0
+      bounded_tracking_error_layer:
+        plugin: "nav2_costmap_2d::BoundedTrackingErrorLayer"
+        enabled: true
+        path_topic: "/plan"
+        look_ahead: 2.0
+        step: 40
+        corridor_width: 2.0
+        corridor_cost: 190
+        wall_thickness: 4
+        cost_write_mode: 2
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: true

nav2_costmap_2d/CMakeLists.txt

@@ -81,6 +81,7 @@ add_library(layers SHARED
   plugins/range_sensor_layer.cpp
   plugins/denoise_layer.cpp
   plugins/plugin_container_layer.cpp
+  plugins/bounded_tracking_error_layer.cpp
 )
 target_include_directories(layers
   PUBLIC

nav2_costmap_2d/costmap_plugins.xml

@@ -15,6 +15,9 @@
     <class type="nav2_costmap_2d::VoxelLayer"     base_class_type="nav2_costmap_2d::Layer">
       <description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
     </class>
+    <class type="nav2_costmap_2d::BoundedTrackingErrorLayer"    base_class_type="nav2_costmap_2d::Layer">
+      <description>Creates a corridor around the path.</description>
+    </class>
     <class type="nav2_costmap_2d::RangeSensorLayer" base_class_type="nav2_costmap_2d::Layer">
       <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</description>
     </class>