MPPI Obstacle bypass critic

nav2_bringup/launch/tb4_simulation_launch.py

@@ -188,7 +188,7 @@ def generate_launch_description() -> LaunchDescription:
     )
 
     declare_simulator_cmd = DeclareLaunchArgument(
-        'headless', default_value='True', description='Whether to execute gzclient)'
+        'headless', default_value='False', description='Whether to execute gzclient)'
     )
 
     declare_world_cmd = DeclareLaunchArgument(

nav2_bringup/params/nav2_params.yaml

@@ -123,7 +123,7 @@ controller_server:
       path_length_tolerance: 1.0
     PathHandler:
       plugin: "nav2_controller::FeasiblePathHandler"
-      prune_distance: 2.0
+      prune_distance: 10.0 #TODO
       enforce_path_inversion: false
       enforce_path_rotation: false
       inversion_xy_tolerance: 0.2
@@ -172,6 +172,7 @@ controller_server:
           "GoalCritic",
           "GoalAngleCritic",
           "PathAlignCritic",
+          "ObstacleBypassCritic",
           "PathFollowCritic",
           "PathAngleCritic",
           "PreferForwardCritic",
@@ -210,15 +211,25 @@ controller_server:
         cost_power: 1
         cost_weight: 14.0
         max_path_occupancy_ratio: 0.05
+        min_distance_occupancy_check: 4.5 # TODO his: 4.5. my horizon is 2.8, so try 3.3?
         trajectory_point_step: 4
         threshold_to_consider: 0.5
         offset_from_furthest: 20
         use_path_orientations: false
+      ObstacleBypassCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.667 #TODO
+        bypass_offset_dist: 1.0 #TODO
+        max_path_occupancy_ratio: 0.05
+        min_distance_occupancy_check: 4.5 #TODO
+        threshold_to_consider: 0.5 #TODO
+        offset_from_furthest: 20 #TODO
       PathFollowCritic:
         enabled: true
         cost_power: 1
         cost_weight: 5.0
-        offset_from_furthest: 5
+        offset_from_furthest: 5 #TODO
         threshold_to_consider: 1.4
       PathAngleCritic:
         enabled: true
@@ -241,9 +252,13 @@ local_costmap:
       global_frame: odom
       robot_base_frame: base_link
       rolling_window: true
-      width: 3
-      height: 3
+      width: 10
+      height: 10
       resolution: 0.05
+      # footprint: "[ [2.230, 0.65], [-1.4312, 0.65], [-1.4312, -0.65], [2.230, -0.65] ]"
+      # footprint_padding: 0.1
+      # footprint: "[ [0.725, 0.525], [0.725, -0.525], [-0.725, -0.525], [-0.725, 0.525] ]"
+      # footprint_padding: 0.12
       robot_radius: 0.22
       plugins: ["voxel_layer", "inflation_layer"]
       filters: ["keepout_filter"]
@@ -255,8 +270,8 @@ local_costmap:
         lethal_override_cost: 200
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.70
+        cost_scaling_factor: 0.7 #TODO
+        inflation_radius: 5.0 #TODO
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: true
@@ -279,9 +294,9 @@ local_costmap:
           clearing: true
           marking: true
           data_type: "LaserScan"
-          raytrace_max_range: 3.0
+          raytrace_max_range: 10.0 # TODO 10.0
           raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
+          obstacle_max_range: 9.5 # TODO 9.5
           obstacle_min_range: 0.0
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
@@ -299,7 +314,7 @@ global_costmap:
       robot_radius: 0.22
       resolution: 0.05
       track_unknown_space: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      plugins: ["static_layer", "inflation_layer"]
       filters: ["keepout_filter", "speed_filter"]
       keepout_filter:
         plugin: "nav2_costmap_2d::KeepoutFilter"

nav2_mppi_controller/CMakeLists.txt

@@ -94,6 +94,7 @@ add_library(mppi_critics SHARED
   src/critics/cost_critic.cpp
   src/critics/goal_critic.cpp
   src/critics/goal_angle_critic.cpp
+  src/critics/obstacle_bypass_critic.cpp
   src/critics/obstacles_critic.cpp
   src/critics/path_align_critic.cpp
   src/critics/path_angle_critic.cpp

nav2_mppi_controller/README.md

@@ -146,7 +146,8 @@ Uses inflated costmap cost directly to avoid obstacles
  | threshold_to_consider      | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered                                    |
  | offset_from_furthest      | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading.  |
  | trajectory_point_step      | int | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable.   |
- | max_path_occupancy_ratio   | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene.  |
+ | max_path_occupancy_ratio   | double | Default 0.07 (7%). Maximum proportion of a path segment that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene.  |
+ | min_distance_occupancy_check   | double | Default 2.0. Min distance for the occupancy check's path segment. The segment length is ``min(min_distance_occupancy_check, distance_to_furthest_reached_path_point)`` |
  | use_path_orientations   | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false.  |
 
 #### Path Angle Critic
@@ -190,6 +191,20 @@ Uses inflated costmap cost directly to avoid obstacles
  | cost_power            | int      | Default 1. Power order to apply to term.                                                                    |
  | deadband_velocities   | double[] | Default [0.0, 0.0, 0.0].  The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action.      |
 
+#### Obstacle Bypass Critic
+
+Steers the robot around dynamic obstacles blocking the path by determining the best side to bypass using the costmap and scoring trajectories toward an offset target point.
+
+ | Parameter                    | Type   | Definition                                                                                                  |
+ | ---------------              | ------ | ----------------------------------------------------------------------------------------------------------- |
+ | cost_weight                  | double | Default 4.667. Weight to apply to critic term.                                                              |
+ | cost_power                   | int    | Default 1. Power order to apply to term.                                                                    |
+ | min_distance_occupancy_check | double | Default 2.0. Minimum distance (m) along the path to check for occupancy to determine if the path is blocked. |
+ | max_path_occupancy_ratio     | double | Default 0.07 (7%). Maximum proportion of the checked path segment that can be occupied before the bypass behavior activates. |
+ | offset_from_furthest         | int    | Default 20. Number of path points after the furthest reached point to place the bypass target, controlling how far ahead the robot aims while bypassing. |
+ | threshold_to_consider        | double | Default 0.5. Distance between robot and goal above which the bypass critic stops being considered.          |
+ | bypass_offset_dist           | double | Default 1.0. Additional offset distance (m) beyond the first free cell when computing the bypass target point perpendicular to the path. |
+
 
 ### XML configuration example
 ```
@@ -316,7 +331,7 @@ Tune std's carefully for low acceleration. If you're seeing a lot of chatter in
 
 If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics.
 
-By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state.
+By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. The ``ObstacleBypassCritic`` can be helpful to bypass around obstacles on the path, if desired. Just make sure to have a reasoanble prediction horizon (model_dt * time_steps), prune distance, and costmap sensor range to see far enough ahead to start going around an obstacle before approaching it to slow down. As such, `min_distance_occupancy_check >= prediction horizon`, in general.
 
 If you want to slow further on approach to goal, consider increasing the ``threshold_to_consider`` parameters to give a hand off from the path tracking critics to the goal approach critics sooner - then tune those critics for your profile of interest.
 
@@ -328,6 +343,10 @@ The same applies to the Path Follow and Align offsets from furthest. In the same
 
 The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline.
 
+If your robot's kinematics is very different than the defaults, you may need to tune ``PathAlignCritic.min_distance_occupancy_check``. This distance defines the min length of the path segment used to disable the ``PathAlignCritic``. A reasonable initial value should be the horizon's length (``= time_step*model_dt``) at full speed. If the distance is too short, the robot may slow down when approaching an obstacle ahead and may even get too close to it and get stuck. If the distance is too big, the critic will be disabled too early, which may cause deviation from path (if for example, the reference point for PathFollowCritic is beyond a curve on the path).
+
+Note: that a sufficiently long horizon length is needed for this behavior to work well. If the horizon is too short, the controller may not find trajectories around the obstacle.
+
 ### Obstacle, Inflation Layer, and Path Following
 
 There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task.

nav2_mppi_controller/critics.xml

@@ -1,6 +1,10 @@
 <class_libraries>
   <library path="mppi_critics">
 
+    <class type="mppi::critics::ObstacleBypassCritic" base_class_type="mppi::critics::CriticFunction">
+      <description>mppi critic for steering around dynamic obstacles blocking the path</description>
+    </class>
+
     <class type="mppi::critics::ObstaclesCritic" base_class_type="mppi::critics::CriticFunction">
       <description>mppi critic for obstacle avoidance</description>
     </class>

nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacle_bypass_critic.hpp

@@ -0,0 +1,69 @@
+// Copyright (c) 2026 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLE_BYPASS_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLE_BYPASS_CRITIC_HPP_
+
+#include "nav2_mppi_controller/critic_function.hpp"
+#include "nav2_mppi_controller/models/state.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+
+namespace mppi::critics
+{
+
+/**
+ * @class mppi::critics::ObstacleBypassCritic
+ * @brief Critic objective function for steering around dynamic obstacles
+ * blocking the path. Uses the costmap to determine which side of the
+ * obstacle is best to bypass and how far to offset from the path.
+ */
+class ObstacleBypassCritic : public CriticFunction
+{
+public:
+  /**
+   * @brief Initialize critic parameters and runtime state.
+   */
+  void initialize() override;
+
+  /**
+   * @brief Score trajectories based on obstacle bypass objective.
+   * @param data Critic data containing trajectories, path, and context.
+   */
+  void score(CriticData & data) override;
+
+protected:
+  /**
+   * @brief Determine the best side and distance to bypass an obstacle using costmap
+   * @param path_x X position on path at obstacle
+   * @param path_y Y position on path at obstacle
+   * @param path_yaw Yaw of path tangent at obstacle
+   * @param[out] signed_offset Signed offset distance (+ left, - right)
+   * @return true if a valid bypass target was found
+   */
+  bool determineBestBypassSide(
+    float path_x, float path_y, float path_yaw, float & signed_offset);
+
+  size_t offset_from_furthest_{0};
+  float threshold_to_consider_{0};
+  float min_distance_occupancy_check_{0};
+  float max_path_occupancy_ratio_{0};
+  float bypass_offset_dist_{0};
+  unsigned int power_{0};
+  float weight_{0};
+  bool bypass_active_{false};
+};
+
+}  // namespace mppi::critics
+
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLE_BYPASS_CRITIC_HPP_

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

@@ -48,6 +48,7 @@ class PathAlignCritic : public CriticFunction
   size_t offset_from_furthest_{0};
   int trajectory_point_step_{0};
   float threshold_to_consider_{0};
+  float min_distance_occupancy_check_{0};
   float max_path_occupancy_ratio_{0};
   bool use_path_orientations_{false};
   unsigned int power_{0};