fix for #1363 - use clearMap instead of reset to clear costmaps

[mlherd]

• This PR is a fix for Planner and Controller server die with message - [PARTICIPANT Error] Type : sensor_msgs::msg::dds_::PointCloud2_ Not Registered #1363.
• Created clearMap function for each layer
• Use clearMap() instead of reset() to clear the costmaps because reset is costly since it does so many unnecessary stuff

[SteveMacenski]

I think this is going to start getting more involved than you bargained for. Why not remove the unsubscribe/resubscribe & replace with a flag in the callbacks to ignore incoming information or something? That seems to accomplish the same thing but without changing the structure of clear/resets

[ghost]

@SteveMacenski Are you ok with these changes now?

[SteveMacenski]

No, I really do not think this is the right way to go, I think it adds complexity and ambiguity.

This issue you're trying to solve is not toggling the subscribers, that should be what is directly solved by replacing that behavior using the enabled characteristics

[ghost]

that should be what is directly solved by replacing that behavior using the enabled characteristics

I don't understand.

I don't see a simpler way to do this other than deleting the old reset function and replacing with these new clearMap functions.

Basically, @mlherd has taken the old function and deleted out the subscribe/unsubscribe and fixed a few bugs in the process.

[SteveMacenski]

Lets follow the trail of how it worked before, and how its changed.

In costmap 2d ROS we'd have a plugin->reset() method. reset() was a virtual method with a default implementation {} in the header.

Then if we look into a specific layer, lets say the static layer, the reset method resets the subscribers undeclares and sets up the plugin again.

Now in this PR, we replace reset() with clearMap() as our entry point for costmap 2d ROS, similarly with a virtual method with default implementation{} in the cpp file.

In the static layer, the clearMap() method will set some parameter, not sure why its here and not in reset() but not really the point.

Ok. So now we have this reset() method that was never removed without anyone calling it, and then clearMap(). That's the first issue I do not like, we're renaming methods and swapping them out, and then not removing the initial implementations. At that point, why make a new method? That doesn't serve any value other than to confuse, we should be changing the implementations of reset(), not replacing the method entirely and then not removing the original. Moreover, none of the resets were removed, so we have a littering of broken, unused methods that look like they should be called (and are consistent with our naming convention of things we use, more on that later).

In the voxel layer, we're renaming resetMaps to clearMaps, I don't know why, but again, less the point. Within the new clearMaps, Costmap2D::resetMaps(); was replaced with ObstacleLayer::clearMap();, which does entirely different things. Within ObstacleLayer::clearMap();, we will eventually call Costmap2D::resetMaps();, but now we're also resetting the buffers which wasn't happening before in the voxel layer. I don't know what the performance or other implications of that are, but seems to be a hidden result of that choice that I don't understand off hand how it will affect things.

Finally, the reset(), resetMap(), resetMaps() is a consistent naming nomenclature across the base class Costmap2D, the costmap plugin layers, the higher level LayeredCostmap, and the application layer Costmap2DROS to do specific and consistent things. Renaming this could potentially break inheritance properties and also breaks some naming conventions.

tl;dr
I have no issue with what this is trying to do, but I see renaming methods as a detriment, leaving old implementations around but unused is confusing, and added behaviors that I dont think have been fully thought about. This is why I recommend only changing the method reset{s, Map, Maps} implementations and not adding some additional methods. The issue we're trying to solve is just to not kill and recreate the subscribers with onInitialize, so just remove the undeclare and the onInitialization and be done with it?

[ghost]

That doesn't serve any value other than to confuse, we should be changing the implementations of reset(), not replacing the method entirely and then not removing the original.

I agree. We should delete the old methods.

In the voxel layer, we're renaming resetMaps to clearMaps, I don't know why, but again, less the point. Within the new clearMaps, Costmap2D::resetMaps(); was replaced with ObstacleLayer::clearMap();, which does entirely different things. Within ObstacleLayer::clearMap();, we will eventually call Costmap2D::resetMaps();, but now we're also resetting the buffers which wasn't happening before in the voxel layer.

This is the part that is fixing some bugs. The old voxel layer was not resetting things cleanly. It bypassed the obstacle layer it was based on, preventing it from properly cleaning up. I believe the new behavior here is much more correct.

---

I'd like to keep the new voxel layer implementation. I believe the old one was broken.

I'm hearing we need to delete the old reset functions and rename the new clearMap functions as reset. This preserves the old interface.

This will have the effect that reset only clears data structures, triggers a refresh of the costmap layer and no longer mucks with the topic subscription.

[SteveMacenski]

I'm hearing we need to delete the old reset functions and rename the new clearMap functions as reset. This preserves the old interface.

Yes, just change the things happening in the existing methods.

This is the part that is fixing some bugs. The old voxel layer was not resetting things cleanly. It bypassed the obstacle layer it was based on, preventing it from properly cleaning up. I believe the new behavior here is much more correct.

I don't disagree it may be a benefit, but that may have been done on purpose in ROS1, I really just don't know. I think that needs to be stress tested.

This will have the effect that reset only clears data structures, triggers a refresh of the costmap layer and no longer mucks with the topic subscription.

Yes, but should be stress tested with clearing a bunch to make sure it doesnt have some weird artifacts happening

[ghost]

Yes, but should be stress tested with clearing a bunch to make sure it doesnt have some weird artifacts happening

@mlherd We tested this didn't we? Before the changes, the voxel layer wasn't getting cleared properly. Afterwards it worked, as I recall. I might be getting mixed up with obstacle layer though. Can you confirm?

[ghost]

FYI: Making the change to alter the existing reset method instead of creating a new one will conflict with #1431. Be careful during the merge.

[codecov]

Codecov Report

Merging #1412 into master will decrease coverage by 0.25%.
The diff coverage is 78.57%.

@@            Coverage Diff             @@
##           master    #1412      +/-   ##
==========================================
- Coverage   40.33%   40.08%   -0.26%     
==========================================
  Files         229      229              
  Lines       11739    11731       -8     
  Branches     5057     5059       +2     
==========================================
- Hits         4735     4702      -33     
- Misses       3614     3644      +30     
+ Partials     3390     3385       -5

Flag	Coverage Δ
#project	40.08% <78.57%> (-0.26%)	⬇️

Impacted Files	Coverage Δ
nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp	70% <ø> (+6.36%)	⬆️
...tmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp	100% <ø> (ø)	⬆️
...av2_costmap_2d/test/integration/obstacle_tests.cpp	33.33% <ø> (ø)	⬆️
...tmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp	63.63% <ø> (ø)	⬆️
...costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp	8% <ø> (ø)	⬆️
nav2_costmap_2d/plugins/static_layer.cpp	33.75% <100%> (-1.22%)	⬇️
nav2_costmap_2d/plugins/voxel_layer.cpp	21.69% <100%> (-2.51%)	⬇️
...map_2d/include/nav2_costmap_2d/inflation_layer.hpp	92.85% <100%> (-0.25%)	⬇️
nav2_costmap_2d/plugins/obstacle_layer.cpp	42.18% <66.66%> (-1.46%)	⬇️
nav2_costmap_2d/src/costmap_2d_ros.cpp	28.9% <80%> (+0.73%)	⬆️
... and 4 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update aa764eb...3552e9c. Read the comment docs.

[ghost]

Since you've had a chance to look at the changes, I'm going to rebase the branch to get it to pass CI.