Add nav_msgs/PoseParticle and nav_msgs/PoseParticleCloud messages

[naiveHobo]

Signed-off-by: Sarthak Mittal sarthakmittal2608@gmail.com

As per ros2/rviz#538 and ros-navigation/navigation2#1623, I propose adding nav_msgs/Particle and nav_msgs/ParticleCloud nav_msgs/PoseParticle and nav_msgs/PoseParticleCloud messages to represent particles/particle clouds produced by particle filters.

[wjwwood]

My main concern here is that the name does not match the contents. In my experience, which is admittedly limited here, there's nothing inherent about a particle in a particle filter that indicates pose with a single weight, even in robotics, and even with navigation (though with robot navigation I assumes common place). If you want this to be specifically for particle filters that use pose and a weight, then a different name is in order, imo. If you want it to be a general purpose particle that can be used with any particle filter then the name is fine, but the contents should be more flexible. I think the way Pointcloud2 is structured (with channels describing binary data) is an appropriate example. Same if you want it to be a particle that can be used with any robot navigation particle filter, except maybe you don't need configurable channels.

For a different name, I'm not 100% sure what it should be.

If the idea is that most all particle filters that are for robot localization would use this message structure (pose + 1 weight), then something like nav_msgs/WeightedPoseParticle and nav_msgs/WeightedPoseParticleCloud could be good, even if verbose. It would be nice to save the names Particle and ParticleCloud for a more generic particle message if we ever need that.

However, this leads me to think that it isn't that generic. I'd be happy to be shown wrong, but to do that I'd like to hear from others in the community that implement nav particle filters on whether or not they would converge on this type. Or maybe if there are other messages out there on the internet and we could collect a few and show how similar they are (and therefore worthwhile to try and standardize them).

If the idea is that this message is just what works for one or more filters in the nav2 project and we want it here to be convenient or to just see if other people use it, then I don't think it belongs here yet and also that the rviz plugin mentioned in the original description also does not belong in the default plugins. I'm not trying to be harsh here, but what I want to do is encourage convergence on common types, but for that to happen the types need to be reusable and need to apply to a decent set of existing or planned works. This is hard work and if no one is interested in doing that then I think it's better to keep the message and plugins in their own repositories until they can be shown to be general. Others may disagree, so please chime in. @ros2/team

[SteveMacenski]

Same if you want it to be a particle that can be used with any robot navigation particle filter

I'm not bringing this up to try to argue your point, see below I think we have some philosophical overlap on naming, but I'd like to temper expectations on what this package is. nav_msgs, to the best of my understanding, has never been a collection of messages suitable for every form of robot navigation. If that were the case, about 2/3 of these messages shouldn't be here. These messages are here to support the work happening in the navigation stack, which often can lend themselves useful to other navigation projects. Effort should be placed in making sure they're reusable when reasonable, but at the end of the day, this is the navigation project's messages and I think its a little different from the rest of the packages in this repo that it acts to serve a specific ecosystem of users rather than generally used messages. I would argue that this package shouldn't be in this repo at all and belongs in ros-perception like moveit_msgs, which is a direct analog. I don't really care where it lives, and I think this is a really great discussion we're having, but I think we're coming at this from different mindsets that we should explicitly acknowledge.

Anyhow.

I think your name suggestions are really good - that brings up another side of your argument that I can agree with. This should be more specific as to the type of particle it is. Personally I think the weighted bit is redundant. In the general formulation of a particle filter, each particle has a score so I think that's totally implied. PoseParticle and PoseParticleCloud I could totally support.

Other examples in the community I found (still MCLs admittedly) which could readily use this definition of the messages proposed, under whatever name we decide on:

• NDT-MCL uses a PoseParticle
• Quick-MCL uses just a visualization_msgs/MarkerArray - no weights
• mcl pi gazebo uses the PoseArray that we're trying to move from to show particle weights
• MRPT localization uses the PoseArray that we're trying to move from to show particle weights
• AMCL3D fails to publish anything at all

You're not being harsh, we're having a conversation - a good conversation :-)

[wjwwood]

Ok, with the presumption that Particle means "state plus a single weight", then PoseParticle* sounds good to me.

[SteveMacenski]

@naiveHobo are you OK with that?

[wjwwood]

nav_msgs, to the best of my understanding, has never been a collection of messages suitable for every form of robot navigation. If that were the case, about 2/3 of these messages shouldn't be here.

I agree with you, but moving forward, if I'm pressed to say "what should be in common interfaces or not" and "what should be in rviz default plugins or not", then I think it should either be "the message is generally useful for running a ROS system" or "the message is general enough to be used by multiple parties for the same purpose". Many of the messages here are here due to historical reasons.

We probably need a "generic nav messages package" and a "ROS nav(2) project specific messages package" to separate the use cases. Like many things in ROS the nav stack's messages were intended to be the standard, but it fell short.

[naiveHobo]

PoseParticle works. I'll go ahead with the changes then?

[SteveMacenski]

We probably need a "generic nav messages package" and a "ROS nav(2) project specific messages package" to separate the use cases. Like many things in ROS the nav stack's messages were intended to be the standard, but it fell short.

We sort of have that, we have nav2_msgs that are messages in a staging ground before eventually being homologated with nav_msgs. Whether that's nav2_msgs go into nav_msgs or nav_msgs gets relocated to ros-perception. The other things in nav2_msgs are still changing and don't belong here (yet). I suspect the actions will never belong here unless we move to ros-perception out of common_interfaces to signal the more specific project nature of it. I'm not pushing for that right now or in the near term future.

In any case, hopefully over the next few years of navigation work, we'll start being able to generalize things better. I'm slowly breaking up alot of the early assumptions and making things more flexible, but it takes time.

@naiveHobo, yes please make those updates.

[jacobperron]

These messages are here to support the work happening in the navigation stack

I've never thought about the nav_msgs package this way before. Though indeed it is written on the wiki and in the README of this package that they are for the navigation stacks. Personally, I've used them in several other projects that have not used any of the other elements of the nav stack (ROS 1). It might be worth considering removing the emphasis on any particular navigation stack.

+1 for renaming to PoseParticle and PoseParticleCloud.

I think it would go a long way to get buy-in from maintainers of the other packages that produce similar data.
We can also elicit general feedback from the community with a Discourse post.

[SteveMacenski]

@wjwwood @jacobperron any other blocking reasons not to approve this now?

[SteveMacenski]

@wjwwood @jacobperron can you please take a look at this again or approve it?

[tfoote]

I have a few specific questions about the structure and areas that need clarification.

There does appear to be a use case for it. However, we typically want messages to have been tested with non-trivial implementations before adopting them into common_msgs/common_interfaces. And this one is an evolution on an existing message which is in that area.

In reviewing the message it didn't really feel like it was a semantically meaningful datatype that can stand on its own. One of the two sub-datatypes doesn't have units defined. And there's no implementation of the subscriber side: ros2/rviz#538 An implementation there would hopefully identify that the lack of units is a problem as it would be necessary to be able to render it effectively.

I also look at this message and it seems that it is purely for visualizing the state of the particle filter. Defining a new message with all the semantic meaning for reuse in common_interfaces, and then implementing a custom rviz plugin seems like a lot more work than simply publishing a MarkerArray with the poses drawn to scale. Leveraging this existing message type means that there is not a need for a custom plugin for rviz, and it will also work in all the other visualizers that already implement MarkerArray support. This was suggested earlier and the advocacy was to give the Particle Cloud 1st class support. But until we have something that would subscribe to the datatype besides the visualization we can't really consider it to be known to be feature complete and thus providing first class status.

A quick browse through the code I see valuable information such as max_particles, min_particles, total weight/denominator of weight. The number of particles compared to min and max seems to effect the interpretation of the cloud. And there are also other parameters of the filter itself that might be valuable to help interpret the particle cloud such as what sort of rates of decay and reinforcement are being applied, that might effect how to interpret the evaluation of the weights. To make this a first class message we'd want it to be self contained and I think the best test of that would be that you could publish it from one particle filter and subscribe to the topic and bootstrap a second filter with the PoseParticleCloud and subscribed to the same inputs and parameters they should produce the same results within some epsilon. Would this message sufficient to do that?

I know that I'm being picky but if we merge it into common_interfaces we are promising that things be very stable we don't want to have to iterate on it after accepting it.

[tfoote]

What is this weight? It needs a semantic meaning and to have clearly defined units so that different implementers don't end up with incompatible semantics.

[SteveMacenski]

See comment below

[tfoote]

Is this message designed to be used standalone without being inside the cloud below? If so it needs more information. If not it should be documented as such.

[SteveMacenski]

Ok, we can document it more, it probably does not have a bunch of value outside of the cloud message, but a weighed pose could have general value in other areas as well.

[tfoote]

If just the collection of the particles enough to communicate everything about the state of the particle filter?

This would probably be a good place to capture more of the cloud parameters. Such as any other components of the state of the particle filter.

[SteveMacenski]

On a non-trivial use - @naiveHobo and I can update the rviz plugin and AMCL to use this version of the message. Its currently using nav2_msgs version of this only because this hasn't been approved by the maintainers here philosophically that it will eventually be merged. If its merged, or even if its clear that it will be merged, we'd be more than happy to update the rviz2 plugin and AMCL to use it. I'd also be more than happy to transition MRPT localization to use it as well, but with similar needs. I want to know that this will eventually be able to be here before I stick my neck out and update MRPT to use it.

On units - weights are unitless normalized quantities in a particle filter s.t. sum(weights) = constant, by convention set to 1, because why chose anything else. Decoupling the RVIZ2 implementation from this PR, we could change the arrows to be representated by a color range 0-1. In practice, most of that range would never be utilized because you never have just 1 or 2 particles, usually its hundreds.

I also look at this message and it seems that it is purely for visualizing the state of the particle filter.

I can tell you from experience that those weights are really important for robust delocalization detection of particle filter based localization approaches. It is not just for visualization. Really, that's what I'm trying to enable here because that's been a common complaint of the nav stack in ROS1 that there's insufficient information to do even remotely reasonable delocalization detection.

A quick browse through the code I see valuable information such as max_particles, min_particles, total weight/denominator of weight. The number of particles compared to min and max seems to effect the interpretation of the cloud.

That is correct, but not every MCL is an A-MCL. To be general for the caass of problem of pose particle filters, we do not assume that it has to be adaptive. I would agree though that more state information like that would be valuable for specific applications using specific MCL techniques. That statement is an argument for an amcl_msgs package containing that information and debug output.

[naiveHobo]

One thing that could be changed here is removing the individual PoseParticle and keeping only PoseParticleCloud message. A single particle with weight doesn't semantically mean anything unless there are other particles to give some meaning to the weights. We can have a list of poses and a list of weights representing individual particles in the PoseParticleCloud message along with more state information about the particle filter.

[SteveMacenski]

So you're suggesting something like:

Pose[] poses
float32[] weights

where they're indexed the same? I think that's less awesome in my opinion but I could live with it if that was better to someone.

Just to take a step back here: we're wanting to put a message into nav_msgs because we really want the rviz_plugin to live in rviz so that it ships for general use. That was a reasonable but slightly arbitrary decision to not add any additional dependencies to rviz2 because adding nav2_msgs wasn't ideal. I would be fine with having something like an amcl_msgs package with a bunch of more context, but that still doesn't resolve the main issue why we're here - we want the rviz plugin shipped by default. Unless you think amcl_msgs could live here, which I don't think it should, and still adds an additional dependency on rviz2. I'm super open to tons of scrutiny in new message definitions, but keep in mind what our actual goal is. We don't really care where this lives and if we add or subtract some scope from it at the end of the day, as long as some set of arrangements allows us to move the display type into the rviz2 project.

Edit: whoops, forgot to mention why I added that context. Point being that I respect some of your concerns. I think 60% of them we can talk through and deal with and the other 40% are concerns that I understand and in some cases agree with. But the context of your reviewing is for inclusion to nav_msgs whereas I'm less concerned by that if we can come up with some compromise that lets us get some weighted-particle-filter message in a place where we can agree to include in Rviz2. Maybe the right answer is that this lives somewhere else, make the good changes you brought up here, and compromise that a new dependency will be added, be it nav2_msgs, amcl_msgs, etc. Just to be explicit that we have multiple dials here we can turn to get the behavior that Sarthak and I are looking for.

[naiveHobo]

where they're indexed the same? I think that's less awesome in my opinion but I could live with it if that was better to someone.

I agree, but I think the PoseParticle message is what will face the most resistance since it is only adding a single field to a pose which in itself doesn't mean anything unless combined with the weights of all the other particles in a cloud. Even if we decide to add more fields for state information, they will go into the PoseParticleCloud message. Removing the PoseParticle message makes a stronger argument for adding the PoseParticleCloud message since this can now individually be used in particle filters which is its intended purpose.

Also, we can go ahead and rename it back to ParticleCloud in that case maybe?

[SteveMacenski]

No, the Pose is necessary. I don't think Tully's comment was that the PoseParticle was bad, I think he just wanted more context in the message definition itself on the uses of it

[sloretz]

@tfoote friendly ping :) looks like there have been changes since your last review

[tfoote]

On units - weights are unitless normalized quantities in a particle filter s.t. sum(weights) = constant, by convention set to 1, because why chose anything else. Decoupling the RVIZ2 implementation from this PR, we could change the arrows to be representated by a color range 0-1. In practice, most of that range would never be utilized because you never have just 1 or 2 particles, usually its hundreds.

This is exactly my point. To be in a standard message these conventions should be standardized. If you use the convention of summing to 1 and I use the convention of summing to 100 our data is incompatible. I'm not saying we can't have the datatype, but we just need to make sure it's able to stand on it's own and consider all the use cases we think are important not just know that it works for one use case and choose it. After something is accepted into common_interfaces generally it should be considered stable and we shouldn't be considering revising it or iterating on it any time soon.

Decoupling the RVIZ2 implementation from this PR, we could change the arrows to be representated by a color range 0-1. In practice, most of that range would never be utilized because you never have just 1 or 2 particles, usually its hundreds.

This would suggest that the rviz plugin could have some different auto-scaling policies.

[SteveMacenski]

@tfoote you're asking that a ROS standard message should be used to standardize the broader field of MCLs, which is not our role to do so. We support the float64 weights which can sum to any random value and as far as the message cares, it doesn't matter. Weights do not have any absolute meaning, their all relative meanings to their distributions.

The other discussions are topics for the rviz plugin which are reasonable. We could definitely allow sums of any number to be the distribution without much effort.

[tfoote]

@tfoote you're asking that a ROS standard message should be used to standardize the broader field of MCLs, which is not our role to do so.

Yes, I'm reviewing these messages not for your application but for what a user would find if they browse through nav_msgs as part of common_interfaces, they should have value on their own or if just echo'd on the command line. The proposed messages do not refer to AMCL, generic MCL or anything else. Those use cases can be used to guide the design and make sure that there's coverage for those use cases.

We support the float64 weights which can sum to any random value and as far as the message cares, it doesn't matter. Weights do not have any absolute meaning, their all relative meanings to their distributions.

With that statement they should never be communicated separately. And if I turn that around and say that since they don't have any absolute meaning in your use case, it should be fine to simply normalize them before publishing them. Thus it will still work for your use case and also provide the ability to use this message for other use cases. In particular if you have two clouds you could meaningfully fuse them if they have normalized weights.

Alternatively you could add to the message information about the distribution to which the weight should be compared to keep it as a self contained message. However if you're sending a large array of these messages, embedding the distribution information into each component significantly inflates the size of the message. Whereas adding the semantic that the weights are normalized across the distribution embeds that information into the type and doesn't require repeating it in the data fields. You would want to add the distribution information to each particle only if it was expected to vary between particle.

[SteveMacenski]

So where do we go from here? Its starting to seem like a DOA request. I see a suggestion for adding the total summed weight to the Cloud message so that the total weight summation is explicitly mentioned, but there are other issues you've brought up that I don't think we have a suggestion that fully satisfies your concerns. Open questions that I have seen

• Weight semantic meaning
• Standard for weight meaning
• MCL not being mentioned in a PoseCloud message
• Having a stand alone Pose
• Additional data that may or may not be available for a large number of filter types

That's basically every line we've suggested. I don't like missing the forest for the trees so I suppose there's a more pertinent question: Thematically, do you see some version of this being accepted? If not, then we can discuss the specifics all we want to death, but doesn't change the position that you don't think this is thematically appropriate. I'd rather be told that than death by committee where we both spend a bunch of time and energy on it.

[jacobperron]

do you see some version of this being accepted?

IMO, some version of the particle cloud message could live in nav_msgs.

After something is accepted into common_interfaces generally it should be considered stable and we shouldn't be considering revising it or iterating on it any time soon.

With this is mind, it might be better for the proposed messages to live in a different package outside of common_interfaces (i.e. nav2_msgs, where they currently live) until they are more mature and then move them here at a later date. I don't know if there is precedence for something like this, but it seems reasonable to me. From my scans of the related tickets, it sounds like the main intention is to use the new weights to improve tooling (e.g. visualization and analytics). It might be good to wait and see if the development of such tooling leads to changes of the initial message definitions.

I'm okay to continue iterating in this PR and landing the message in common_interfaces, if other team members are on-board.

[SteveMacenski]

That's helpful, thanks. I suppose it might be helpful if we were to bring up problems and potential tangible solutions to those problems. I'm all for more docs, more clarity, and either attempting to be more general or more specific. I think I need some help in how to turn the issues being brought up into progress.

As to your second comment, we do have the message in nav2_msgs that we can update with the things we did here as well (and the rviz plugin to do autoscaling). I don't see as part of that work any other necessary changes, except adding a "total_sum" field to avoid iterating over each particle to sum values for scale. So if there's other suggestions of changes, docs or specifications to add, etc, I think the discussion here more or less continues per usual :-)

[SteveMacenski]

Pinging on this - I'd really like to have this continue to move forward. Its blocking non-trivial improvements across the ecosystem.

[mikeferguson]

As one of the maintainers of ROS1 Navigation for the past 7 years, as well as someone who has developed several commercial mobile robots, I figured I would pipe in when I saw this.

The messages in nav_msgs are not generally generic enough for every use case: they mainly target 2d navigation (which, is a pretty big use case). For instance, all of the map messages are only good for 2d. This message pair seems to be at least as generic as that (hey, it even supports 3d!) - and if we consider primarily 2d navigation, most robots are running some variant of an MCL particle filter for localization. That particle filter generally consists of a set of weighted particles.

I can tell you from experience that those weights are really important for robust delocalization detection of particle filter based localization approaches. It is not just for visualization. Really, that's what I'm trying to enable here because that's been a common complaint of the nav stack in ROS1 that there's insufficient information to do even remotely reasonable delocalization detection.

I agree here. I've seen that change done several times. It was never top priority to get it merged back into the open source, hence why ROS1 doesn't support it.

I would also agree with some above that the Particle message doesn't really stand on it's own - but without it, the ParticleCloud message would be quite nasty (unless there is some ROS2 feature I'm not aware of that let's us define Particle inside the ParticleCloud message).

So, in conclusion, is this the absolute most important feature to ever get into ROS2? No, but I think it would be a pretty good improvement towards standardizing the messages used in ROS2 localization systems.

[SteveMacenski]

ros-navigation/navigation2#1677 (comment)

There's a clear pattern that companies are hacking messages to get particle weight information due to a lack of a standard message. This now represents another company that have had to make modifications due to a lack of a standard message.

[tfoote]

I do think that a version of these messages can live in nav_msgs, and I would like to see that. I've identified areas where the proposed message is not clearly defined.

The message must be meaningful and documented fully so that if I come across the message definition I can interpret every single component of the message in the same way that everyone else is.

Weight semantic meaning
Standard for weight meaning

If an apple is labeled as 23 without units. You can only get value out of that label by guessing it's unit. If we just agree that the weights of points in the particle cloud sum to 100% it's very easy to interpret the weight of a single sample when processing. And you don't have to sum up all the other particles in the cloud to have a sense of what the weight represents.

The weights effectively are forming a discrete weight function to influence the results of a computation based on the inputs. This is just a semantic meaning that needs to be documented. But while were there we can normalize the weights like we do for most other

This is the most fundamental part of a message definition It needs to describe what this field means, so that an implementer can know how to interpret the values of a given weight. What does a weight of 10 mean? It would be trivial to put into the comments that with weights are the relative importance of any particle on the range 0.0 to 1.0 and sum to 1.0 in a ParticleCloud. This gives you a clear meaning for the meaning of the weight on any given particle, it's self contained and weights have consistent deterministic meaning for a specific particle that can then even be compared across instances of clouds.

MCL not being mentioned in a PoseCloud message
Having a stand alone Pose
Additional data that may or may not be available for a large number of filter types

We should define what use cases we want to cover and which ones we don't want to cover. If we make a super generic "standard" message that doesn't actually cover any use cases then we've defeated the goal of being useful for code reuse. This proposal doesn't call out the specific use cases to cover and thus it's very hard to evaluate the suitability of the message to cover the desired use case(s) for their use. If we're all imagining different use cases in our discussions then we're likely going to talk past each other. Once we understand what the use cases specifically are then the messages can be evaluated as to whether they're meeting them.

I'm coming to this as a non-expert in the navigation field. That's for whom these messages need to be documented so that there's no ambiguity that needs an expert. I hear that there's pressure to do this and people are forging their own way. That's actually a really good way to collect the use cases, find those collect them here and then make sure that we're covering what they need. I don't think that these messages are very far off. But if we're going to stamp them as standard I do really want to make sure that they're actually standard. We made PointCloud and then made PointCloud2 a few months later. And it took us close to 10 years to retired PointCloud. I don't want to do that again.

[SteveMacenski]

I think this is alot easier if we talk in specifics.

PoseParticle - as of now

# This represents an individual particle with weight produced by a particle filter
geometry_msgs/Pose pose
float64 weight

PoseParticleCloud - as of now

# This represents a particle cloud containing particle poses and weights
std_msgs/Header header
# Array of particles in the cloud
PoseParticle[] particles

---

PoseParticle - Proposed to resolve concerns

# This represents an individual particle with weight produced by a particle filter
geometry_msgs/Pose pose # pose of the particle in the PoseParticleCloud
float64 weight # unitless weight of a particle in the PoseParticleCloud

PoseParticleCloud - Proposed to resolve concerns

# This represents a particle cloud containing particle poses and weights
std_msgs/Header header
# Array of particles in the cloud
PoseParticle[] particles # an array of particles in the cloud
Float64 particle_weight_sum # total sum of all particle's weights

This annotates all the values to tell people exactly what's going on and mentions units on weights and the total sum of the weights to be used as appropriate. By default, a reasonable thing to do would be to have the weights sum to 1, but that will not always be the case. It may be reasonable to consider each particle as a pdf containing itself a value between 0-1 and simply scaling them isn't appropriate for the application (or really matters at all).

Is there any modifications you would like to make or request in addition? I think the "Pose" in front of particle / cloud makes it clear what the type of particle is; could be full SE3, SE2, or 2D. The weights are described as unitless, as they are for most every particle filter variants I'm aware of, with a total sum to scale them to a normalized value if necessary.

[hidmic]

@tfoote friendly ping

[SteveMacenski]

@tfoote nudge from 2021 :-)