Improvements in RemoveInCollisionGoals and adjacent features

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu,
Robotic platform tested on	custom gazebo simulation
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Change GetCosts to use PoseStamped instead of Pose2D so that we can use the frame_id to transform the pose in the costmap frame
• Added consider_unknown_as_obstacle to control whether NO_INFORMATION is treated as "in collision" or not
• Make GetCosts service return 255.0 instead of -1.0 if the coords are out of bounds
• fix rviz costmap tool to use the global fixed frame of rviz

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[tonynajjar]

There is still one thing I'd like to modify in this PR but would like your opinion because it might affect a lot of things upstream. About the implementation of footprintCost, if the footprint is partially in free space and partially outside of bounds, then the cost is LETHAL_OBSTACLE. However, in the case of track_unknown_space = false (considered as free space), shouldn't footprintCost also take this into consideration and return FREE_SPACE?

[mergify]

@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

No major notes, just details. This doesn't appear to compile, so once this does its an easy merge

[tonynajjar]

@SteveMacenski can you also comment on my question about footprintCost, I'd like to address it in this PR as well

[SteveMacenski]

Sorry, I missed that.

if the footprint is partially in free space and partially outside of bounds, then the cost is LETHAL_OBSTACLE.

If we can't convert world->map because out of bounds, that's an error case. For example, if we have a line segment of the footprint that's 1 meter long. Only the very last point is off the map, but there is a collision along its length at 0.5m from the starting corner.

In this case, we can't raycast to find the collision (unless we march back along the ray to find when it starts being valid again, and collision check up to then). Imagine now that there was some non-lethal cost 154 along the edge and the corner went out of the map. Should we return unknown or 154?

I'm open to changing behavior here, but its not clear to me even if we refactored this code to march as long as it was valid for (and then take unknown for the rest), what we'd return if any part of the edge is non-zero.

However, in the case of track_unknown_space = false (considered as free space), shouldn't footprintCost also take this into consideration and return FREE_SPACE?

track_unknown_space is about unknown as the background for the costmap (ie before receiving sensor measurements, should we consider the cost free (0) or unknown (255) to be not understood until some free space is raycasted out by sensor measurements), not off the costmap. I don't interpret track_unknown_space as applicable for off the map itself -- but I suppose that could be a not-so-unreasonable assumption to expand into.

However to do so, we need to answer questions like what I posed in the comment block above & how we handle these situations. We need to make sure we set up policies for collision checking off the map & partially off the map & where off the map is never OK but where the background on the map is tracked as unknown.

[codecov]

Codecov Report

Attention: Patch coverage is 88.23529% with 2 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...lugins/action/remove_in_collision_goals_action.cpp	83.33%	1 Missing ⚠️
nav2_costmap_2d/src/costmap_2d_ros.cpp	80.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...lugins/action/remove_in_collision_goals_action.hpp	100.00% <100.00%> (ø)
nav2_costmap_2d/src/layered_costmap.cpp	96.46% <ø> (ø)
...lugins/action/remove_in_collision_goals_action.cpp	85.71% <83.33%> (-3.18%)	⬇️
nav2_costmap_2d/src/costmap_2d_ros.cpp	89.07% <80.00%> (+0.54%)	⬆️

... and 4 files with indirect coverage changes

[tonynajjar]

Okay it's more complicated than I anticipated. I'd like to find a way to solve my immediate problem without too much refactoring then.
My use case is that in the BT I run RemoveInCollisionGoals on poses both on the global and the local costmap.
if the pose is outside the local costmap (or at the border and we are using use_footprint), then the cost is respectively NO_INFORMATION (with this PR) and LETHAL.
I don't want RemoveInCollisionGoals to remove these poses because they are not for sure in collision -> it's unknown.
For NO_INFORMATION, I added consider_unknown_as_obstacle as a workaround but for the footprint cost LETHAL I can't see a solution -> that's why I was proposing to change it.

Any recommendations for solving this without major refactorings? I don't think it's logic that I could introduce in the BT node because we don't have the necessary info about the costmap.

[SteveMacenski]

stamp?

[SteveMacenski]

Okay it's more complicated than I anticipated.

Yeah :/ The system wasn't designed with 'off the map' being a valid or highly considered case. They would have made an OFF_THE_MAP enum type if that was considered.

I don't want RemoveInCollisionGoals to remove these poses because they are not for sure in collision -> it's unknown.
For NO_INFORMATION, I added consider_unknown_as_obstacle as a workaround but for the footprint cost LETHAL I can't see a solution -> that's why I was proposing to change it.

So if you're off the map, you return NO_INFORMATION and you have handling for NO_INFORMATION to make that OK in this PR with a parameterization. If its LETHAL, that's still lethal to skip. I don't understand the issue, that seems to do what you want it to do?

W.r.t. collision checking footprints and part is on the costmap and part is off the costmap that we previously discussed, I suppose that's why this is in the BT to execute on occasion as the task progresses? Once in proximity of that goal, it should be fully on the costmap again to proceed or skip based on its LETHAL or OK known-information state. I think this handles itself due to the rolling costmaps

[tonynajjar]

If its LETHAL, that's still lethal to skip. I don't understand the issue, that seems to do what you want it to do?

I don't want to skip this pose, the cost is LETHAL but only because that's what footprintCost returns for footprints at the border of the costmap when in reality the footprint is on FREE and "OFF_THE_MAP" -> I'd rather consider this as non-lethal

W.r.t. collision checking footprints and part is on the costmap and part is off the costmap that we previously discussed, I suppose that's why this is in the BT to execute on occasion as the task progresses? Once in proximity of that goal, it should be fully on the costmap again to proceed or skip based on its LETHAL or OK known-information state. I think this handles itself due to the rolling costmaps

Exactly, the problem arises when all the close waypoints are removed because of LETHAL (rightfully so because of a real obstacle) and the only "valid" waypoint left is the one at the border of the costmap -> so this one is also removed -> empty list of poses is sent to ComputePathThroughPoses -> planner rightfully throws an error

[SteveMacenski]

Is it possible to know before collision checking if the goals are off the map? We could have another bool parameter like what this PR proposes in the BT node for how to interpret such cases. Either in the BT node:

  for (const auto & goal : input_goals_) {
    request_->poses.push_back(goal);
  }

Or in the service itself, we have some off-map designator only at this higher level BT. Perhaps change the message to allow the population of another field about the on-mapped-ness status. I don't love having costs and XYZ, so if we can combine those, that would be nice. The costs are in float32 and not in the 0-255 that the cost map is stored in, so we could have some value we assign to off map (-1). That feels like abusing an already questionably incorrect type, but one option.

[tonynajjar]

Is it possible to know before collision checking if the goals are off the map

I see isOutofBounds exists but that's only to check if a point is off the map not if a footprint is completely/partially off the map. Is there one? If so, then we can just use it in the service, we already do check that here and that works well for point costs.

While that would solve my immediate issue, I don't think it's good design for the service to do its own on-mapped-ness check and return a cost different than what footprintCostAtPose and getCost would return -> given the same coordinates, a node using the service would get a different cost than another node using the costmap directly

[SteveMacenski]

There is not one currently, we'd need to make one and expose it. The in_bounds check would handle some of the cases (others obviously not in the center is in the costmap but an edge isn't), though. So I suppose from the API level, we can use that to define how we want to communicate that out-of-mapped-ness, that also works for the point-costs. Then, we need to figure out the tech details of the collision checker object to do the same. At least it decouples the problem a bit.

A couple of questions:

• How long are these requests and the number of poses within them?
• How far away are some of these goals off the map?
• Is it important that you understand the collision state of poses 2-3-4-5 poses down the line from the current task?

What I'm thinking is that you only feed into this the next N rather than all M poses, where you can make some guarantee that the next N will be within the rolling costmap's bounds based on regular spacing or known distances from the current origin. If you don't need to know the state of pose N+5 or above, you could simply not check them until within relevant proximity that would promise it to be fully within the costmap's bounds.

If you plan to mark partially-off-map poses as "OK" with lack of information, I figure this has the same behavioral outcomes with less than a third of the technical difficulty. We just need a new BT node to extract only the first N poses or pose within N distance of the current robot pose (or have that logic be part of the remove in collision goals BT node, which also seems rational to set a limit of where we want to check)

[tonynajjar]

Thanks for the proposals, unfortunately I can't justify spending much more time on this for now so I'll take it as accepted limitation for now and will find some hacky solution on my side. This can be merged from my side. Here is the doc for the new param: ros-navigation/docs.nav2.org#594

[SteveMacenski]

@tonynajjar due to the changes in the interface, this is a hard no-fudging-allowed change that cannot be backported to Jazzy. Would you like to propose a Jazzy-compliant similar change to this (if you want this in Jazzy binaries)?

[tonynajjar]

I personally don't really mind not having it in the jazzy binaries, I build a long-term fork.

[SteveMacenski]

Ok! Just checking since this is one of the first main branch things that haven't been able to be backporte to jazzy :-)