Discussion: modularizing nav2_collision_monitor

[tonynajjar]

Feature request

Feature description

Great work on the collision monitor, I've been looking into the implementation and was wondering whether we could detach/modularize it into two parts to use them separately.

The Sensing: detection of an obstacle inside a polygon using different sources. I could imagine this part being a standalone package to be useful for other applications, e.g. activating LEDs if an object comes within range
Basically a node that subscribes to the sources and output a custom msg that contains the information for which polygons are triggered.

The Action: subscribing to part 1 and limiting the velocity accordingly

Do you see part 1 as generally useful? We could ask that question on ROS Discourse.

[AlexeyMerzlyakov]

Hi, @tonynajjar.

From the perspective of use-cases that it would be a nice functionality expansion, to have general information about shapes triggered that will be back to user; and action based on the decision of what we should do in any particular case in a separate logic. The set of additional use-cases for that would be activating LED-s, making warning/information signals, telling the operator what is going on, etc... So, this might be a nice-to-have solution, if it won't brake current functionality/performance of CM.

From the perspective of the design and a quick glance: it could be a separate topic with publishing of triggered shapes; or shared library function call. Currently the CM is sticked the to work with cmd_vel_in velocities for minimum input lag, and we might be restricted by them (for cases if someone want to acquire the shape triggering information with stable frequences, or independently from cmd_vel topic). Anyway, I am not sure this is the killing use-case or some principal restriction, so publishing some topic even with the same as cmd_vel rate case should be a suitable option.

@SteveMacenski, do you have more thoughts on this?

[SteveMacenski]

I would like to not have them separated into separate nodes mostly because of the safety-ish nature of this node, but I totally see your point that the checks in a polygon are very useful for other folks as well. I think it would be sensible to move some of that logic into abstracted utilities in nav2_utils that would then be available for anyone to make use of for building their own applications.

[tonynajjar]

I think it would be sensible to move some of that logic into abstracted utilities in nav2_utils that would then be available for anyone to make use of for building their own applications.

This would already be useful for many people but I think the most accessible and easily-integrable form would be a standalone node.

I would like to not have them separated into separate nodes mostly because of the safety-ish nature of this node

Then again this makes sense for this particular application.

Perhaps we can have both: move the logic to abstracted utilities in nav2_utils and make a node using these utilities. CM would stay as is, just change it to use the utilities.

[SteveMacenski]

Yeah, that would make sense. I'm not sure where though we'd have this standalone node

[SteveMacenski]

Any thoughts where that could live? Potentially even outside of this repo?

[tonynajjar]

I guess it's not necessarily a great fit for Nav2, to my mind it's independant enough (I do still wonder why e.g. the BT ROS wrapper or amcl is still in Nav2)

On the other hand, browsing around in the ROS ecosystem, I struggle to find another suitable place. Perhaps a new repo in https://github.com/ros-perception?

[SteveMacenski]

I suppose we could add it as a node in Nav2 utils, its not like there haven't been other standalone nodes there historically. I just don't like to leave things around that aren't going to get immediate use.

ros-perception could work -- that then asks questions like

• what exactly is going to be supported?
• who's maintaining it?
• documentation / demos / examples?

[AlexeyMerzlyakov]

Hi! In its current state, Collision Monitor has the Polygon and Source (and all their derived classes) to be operate independently from CM node logic itself. They have the following main API (I do not take into account auxiliary functions like getName() or updatePolygon(). They indeed also required, but not the subject of today's discussion), which is current in-use by CM node and which could be used (I suppose) by other developers in their projects:

int Polygon::getPointsInside(const points);
<- used to get the number of points which already inside polygon. Externally used for STOP and SLOWDOWN models.

double Polygon::getCollisionTime(const points, const velocity);
<- estimates the time before collision. Used for APPROACH behavioral model.

void Source::getData(const curr_time, out_points);
<- add all points from the given data source to the "out_points" 2D array.

This API is enough to build the CM node logic and should be enough to build any other logic (e.g. LED as we've discussed above) you can create upon it. So, the separation slice, we are talking about, could be started from this point.
At least this API already could be used to some logic, and we could thing how it could be more unified in order to get better formula.
And therefore, this solution will have a performance and simplicity advantage over the publishing topics with polygons triggered.

So, developing this idea, the separation is likely to be utilized Polygons/Sources classes + API calls from them to use some monitoring logic. I personally, do not prefer to move this logic to nav2_utils for two reasons:

1. nav2_utils usually contains very common for the whole Nav2 stack utilits/classes/etc..., but in our case the area of application of Polygon and Source classes is very restricted by CM and surrounding logic. I still doubt that someone else will use this logic in Nav2.
2. All these use-cases are also intended for collision monitoring. Yes, they are used not the stopping of the robot or approaching it, but also for notifying user, switching the LED, triggering some logic or other similar scenarios. But anyway, all these scenarios could be unified and called as "Collision Monitoring" concept, independently what end-action for robot do we have. In this case I'd like to rename current "Collsion Monitoring" logic into "Collision Safety" or something else (it is being discussable). But the main intention of nav2_collision_monitor Nav2 package will remain the same, even if new functionality to be added there.

Regarding movement of this package into separate project in ros perception, it will definitely work. But as @SteveMacenski mentioned, I have the same questions, like who will be responsible for this package and how it will be maintaned? So, in my opinion, we could plan to remain it inside Nav2 stack, as it is for today. But if we will think about separation logic (moreover, like AMCL and other packages separation), this might be a good subject for another separate issue.

To summarize-up: I suggest to make the separation into two parts (agree with naming as @tonynajjar suggested):

• Sensing part: here we have Polygon and Source classes, their main API (and all child classes for sure) that developers will use
• Action part: one ROS-node per each action logic. Having currently developed logic that could be called "Collision Safety", and other future developments, like "LED" or "Collision Warn", etc... nodes.

The one question still remain open here for me: is what should we do with this all countless set of ROS-parameters belonging to Polygon and Source classes, and being set to the referenced node in there?

[SteveMacenski]

Once there's an application that can make use of this work, we can break it out - but going to close for now since we don't have something along those lines currently planned and its equally as easy for a user to grab the library from nav2_collision_monitor to use as from nav2_utils. If there are any refactoring changes in the current package to make that easier, happy to discuss.

[tonynajjar]

Once there's an application that can make use of this work, we can break it out

We might have an application coming up soon, will get back to you