Create BT action node RemoveInCollisionGoals

[tonynajjar]

Feature request

Feature description

Similarly to the existing RemovePassedGoals, I propose creating RemoveInCollisionGoals that will make use of the collision checkers to remove goals that are in collision.

Context: I have an external node that generate waypoints to follow and calls ComputePathThroughPoses. That node is not costmap aware so I'd like to remove waypoints that are in collision before computing the path.

@SteveMacenski anything against that? Maybe there is an argument for implementing this directly in the planner_server?

Implementation considerations

[SteveMacenski]

Sure thing! The only implementation consideration is where is it done, since working on the global_costmap is quite the data structure and opening up more subscriptions to it should be avoided, if possible and clean.

Maybe planner server? Maybe actually just in the costmap itself as a service option to validate points? We have that new GetCost service, maybe that can be used under the hood for the BT node without any modifications to planner/costmap ...

#4546

[tonynajjar]

opening up more subscriptions to it should be avoided, if possible and clean.

Ah, I thought I could use local_collision_checker_ and global_collision_checker_ from the behavior server but maybe it doesn't make sense for the RemoveInCollisionGoals server/service to be a behavior like wait, spin, etc... I'm just wondering why not? Right now it would feel out of place because these are behaviors that generate movement. Any other reason?

[SteveMacenski]

I'm just wondering why not? Right now it would feel out of place because these are behaviors that generate movement. Any other reason?

Those are mostly actions that move the robot to create a behavior. That does seems a little weird, but not out of the question. Seems like something in the planner server might be more natural in that case. We have an isPathValid method, wouldn't be much to add an areGoalsValid eq.

Also, if there's already a GetCost service in the costmaps, can't the BT node just call that to get the cost at the goal and gauge if its occupied to remove it? It seems like everything you need is already implemented, just need the BT node to call it. Not sure why it would need to be any more complicated.

[tonynajjar]

Alright, I'm on it, I'll use the GetCost service

[SteveMacenski]

We could also modify it to get costs (plural) so you don’t need multiple calls