Full footprint collsion distance in MPPI obstacle critic and use of collision_margin_distance. about navigation2 HOT 14 OPEN

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!

from navigation2.

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

from navigation2.

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.

from navigation2.

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.

from navigation2.

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

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

from navigation2.

yes but pose_cost is always center point cost.

from navigation2.

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.

from navigation2.

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

from navigation2.

      } 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.

from navigation2.

 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.

from navigation2.

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

from navigation2.

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.

from navigation2.

@mangoschorle any update? Happy to have these changes made

from navigation2.

@mangoschorle Just pinging again on this!

from navigation2.