As discussed in PR #3878 , here is an Issue to start a discussion about the obstacles critic.

Feature discussion

The ObstaclesCritic is a critic to increase trajectory score based on the distance to obstacles. With this critic, the MPPI will prefer trajectories that avoid obstacles. It works well but some points can be discussed.

• The obstacles_critic uses the cost from inflattion_layer to compute distances by using the invert function of the cost.

It makes inflation_layer cost_scaling_factor tunning useless, it's good because it reduces tunning and it may be dangerous as it has a quantization effect on the distance. It also increases the computation load.

• The score is computed differently when the robot is far from obstacles and when it is close.

      if (dist_to_obj < collision_margin_distance_) {
        traj_cost += (collision_margin_distance_ - dist_to_obj);
      }
      if (!near_goal) {
        repulsive_cost[i] += inflation_radius_ - dist_to_obj;
      }

The repulsive cost uses the inflation radius that is not related to safety distance. The traj_cost is computed but only used if trajectory_collide.

• If the footprint has some points in the inscribed area, the score is the same regardless of the distance to the obstacle. Because footprintCostAtPose returns the highest cost which will be "INSCRIBED" whatever the distance to the obstacle, distanceToObstacle(cost) will return the same distance so the score will be the same in a situation where it is important to evade obstacles.

Proposed alternative

The proposed InflationCostCrtic is a simplified version of the ObstaclesCritic;

• It copies all its features
• It doesn't compute distances
• It directly uses the costmap cost repulsive_cost[i] += pose_cost.cost
• It uses a fixed critical score if close to an obstacle

This critic benefits from the exponential factor of the inflation layer, it creates a behavior where the robot is forced in the middle of narrow corridors but can easily evade obstacles in wider spaces. It will be able to take shortcuts from the path without going too close to corners.

Here are the default parameters:

InflationCostCritic:
        enabled: true
        cost_power: 1
        repulsion_weight: 0.015
        critical_cost: 1000.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 1.0

Implementation considerations

I will create a PR to share my code but it is not up to date this the latest commits.