Full footprint collsion distance in MPPI obstacle critic and use of collision_margin_distance.

[mangoschorle]

The distance to an obstacle computation (if using full footprint checking) is lower bound by the inscribed radius.

The inflation layer therefore doesn't support distance computations for a full footprint in situations where it would be interesting (e.g. driving along walls).

For example, irrespective of the orientation of the red footprint we would get the same distance result (=inscribed radius, in this case many times larger than the actual distance) and even though we might get arbitrarily close to an obstacle, collision_margin_distancewould never be reached and the assumed distance would be the inscribed radius (in this case 0.6m).

My point being: I fail to see the use of the footprint distance checking. Imho would be of great help, but only given that we computed the actual distances.

Do I have a knot in my head, or does it make sense what I am saying here. Does it make sense to try to calculate the actual distances efficiently?

[SteveMacenski]

My point being: I fail to see the use of the footprint distance checking. Imho would be of great help, but only given that we computed the actual distances.

Its about checking for collisions and less to do with finding the distance. The ObstacleCritic has 2 components: the "critical" collision rejection of samples that collide and the "behavioral" element which scores higher cost, but valid, areas higher to generally incentivize actions that aren't getting into unnecessary high cost areas where there are other options. The collision check is to find the highest cost on the footprint to checking if its valid - then we can use that same information to find then the closest distance to score.

We actually have a different critic now the CostCritic which is now default that doesn't use distances but rather fully based on costs. It falls into the same trap as the ObstacleCritic since the 253 cost is still set for all inscribed values, but it does have some nice behavioral advantages for some - if not most - situations (notably: can tune obstacle behavior relative to the inflation exponential function rather than linear on distance).

Does it make sense to try to calculate the actual distances efficiently?

Perhaps, yes. I suppose for the local costmap given its smaller that creating a TSDF or similar wouldn't be too bad. It was just something I was trying very hard to avoid for computational reasons, but maybe I was overblowing it in my mind. Its definitely worth an experiment. Moreover, if we had a generic TDSF utility for costmaps in Nav2, I'm sure it would find utility for planning, control, collision checking, etc even if I can't put my finger on exactly how I'd use it off the top of my head.

An interesting topic indeed!

[mangoschorle]

The cost critic has some lines that make no sense to me.

https://github.com/ros-navigation/navigation2/blob/df2d686548ca7ba97772c681907a5b90ee01341f/nav2_mppi_controller/src/critics/cost_critic.cpp#L172C1-L174C8

Will pose_cost >= 253.0f ever be reached here?

      if (pose_cost >= 253.0f /*INSCRIBED_INFLATED_OBSTACLE in float*/) {
        traj_cost += critical_cost_;
      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles

Lets say the trajectory actually collides then we exit the loop here

If the trajectory does not collide, then pose_cost will certainly be less than 253, since pose cost is the cost of the origin using costmap->getCost(getIndex(x_i, y_i)). If hypothetically it would be >= 253 then we would be in collision and not near-collision.

pose_cost >= circumscribed_cost_ would make more sense to me

[SteveMacenski]

Will pose_cost >= 253.0f ever be reached here?

253 is inflated, so that happens, yes when checking footprints and not circular point checks. 254 no because that's collision and inCollision will reject it. 255 can happen if tracking unknown space. Its a nice condensation from checking 253 or 255.

[mangoschorle]

253 is inscribed_cost

if pose_cost is >= 253, a collision irrespective of footprint orientation is guaranteed.

If this is the case than footprintCostAtPose in inCollision will certainly return LethalObstacle because one of the footprint`s polygons will be in >=254.

In this case inCollision returns true and the loop will break before reaching the point.

[SteveMacenski]

if pose_cost is >= 253, a collision irrespective of footprint orientation is guaranteed.

For center point costs, not edge costs for SE2 footprint checking.

[mangoschorle]

yes but pose_cost is always center point cost.

[SteveMacenski]

Ah, I was just looking at that. Throw a log in there and make sure it never triggers, but I agree with you that on a glance I think that is dead code.

[mangoschorle]

Isn't that the whole point?
skip expensive checking if its >= inscribed because irrespective of orientation you will collide
skip expensive checking if its < possibly because irrespective of orientation you will not collide

[SteveMacenski]

      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
        traj_cost += pose_cost;
      }

This part is what is doing the scoring based on preference

        if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
          traj_cost = collision_cost_;
          trajectory_collide = true;
          break;
        }

This part is what does the scoring based on core collision

      if (pose_cost >= 253.0f /*INSCRIBED_INFLATED_OBSTACLE in float*/) {
        traj_cost += critical_cost_;

I'm guessing that this part is a legacy of the ObstacleCritic where the pose cost was not just the center point cost. But, if you're saying that this never triggers, then we should be able to remove it.

[mangoschorle]

 if (pose_cost >= 253.0f /*INSCRIBED_INFLATED_OBSTACLE in float*/) {
        traj_cost += critical_cost_;

Once you reach this point and you had a SE2 collision check, you could still just penalize based on > circumscribed_cost_, but it may be a poor indicator if for example you are using a 20m long and 3m wide AGV.

[mangoschorle]

Or even better: you could return the result of score_cost = static_cast<float>(collision_checker_.footprintCostAtPose( in the inCollision function. If this is inside inscribed then you have reason to apply critical_cost

[SteveMacenski]

Once you reach this point and you had a SE2 collision check, you could still just penalize based on > circumscribed_cost_, but it may be a poor indicator if for example you are using a 20m long and 3m wide AGV.

I think like we discussed this can be removed? If you show this isn't called and I generally agree that's probably the case, we can simply remove it.

[SteveMacenski]

@mangoschorle any update? Happy to have these changes made

[SteveMacenski]

@mangoschorle Just pinging again on this!