Open mangoschorle opened 4 months ago
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!
The cost critic has some lines that make no sense to me.
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
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
.
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.
if pose_cost is >= 253, a collision irrespective of footprint orientation is guaranteed.
For center point costs, not edge costs for SE2 footprint checking.
yes but pose_cost
is always center point cost.
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.
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
} 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.
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 > circumscribedcost, but it may be a poor indicator if for example you are using a 20m long and 3m wide AGV.
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
Once you reach this point and you had a SE2 collision check, you could still just penalize based on > circumscribedcost, 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.
@mangoschorle any update? Happy to have these changes made
@mangoschorle Just pinging again on this!
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_distance
would 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?