bugfix (#3109) deadlock when costmap receives new map

[daisukes]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3109
Primary OS tested on	Ubuntu22.04 on docker
Robotic platform tested on	test code

---

Description of contribution in a few bullet points

• fix the deadlock issue when costmap receives a new map
  • tested with a test code https://github.com/daisukes/nav2-costmap-deadlock-test
• as suggested by @SteveMacenski Deadlock when a static layer gets map updated #3109 (comment), processMap is called in incomingMap only for the first map
  • if the first map is buffered, some will fail
• since map is processed sequentially, update_in_progress_ is removed

Description of documentation updates required from your changes

N/A

Future work that may be required in bullet points

• backporting

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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

[mergify]

@daisukes, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.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

[SteveMacenski]

@gimait can you take a look at this since you added that logic to make sure you think this is sensible? To deal with a deadlock issue, we're moving processing of maps always at the start of updateBounds so that there's sequential execution of map updates with the costmap processing. The only exception is for the very first map we receive just so we get up and running faster

[gimait]

I think this is great! We should have moved the processing of the map to the main loop from the beginning, all the threading hell we had here disappeared.

My only comment is that it would be great to remove that last call to processMap from incomingMap.
I also think that we should retest #1625 just in case we are missing something in the review, but I don't see how this might have been broken.

[gimait]

This will probably never be a problem, but please note that part of the processMap method is not thread-safe if we don't lock the Costmap2D mutex here as well. There is also a second call to this mutex within processMap, which might be able to produce the same deadlock that we are fixing.
The best way to go would probably find a way to move processMap to the main thread completely.
Again, this will probably never be a problem, and I might be wrong about this, but just in case.

[daisukes]

Actually, I removed it once, but I got some errors in the test.

[133.395s] The following tests FAILED:
[133.395s] 	 15 - test_collision_checker (Timeout)
[133.395s] 	 17 - inflation_tests (Timeout)

These tests use LayeredCostmap and StaticLayer, but do not call updateMap and wait for the costmap to be ready. So they got a timeout.

So I needed to keep this call here and keep it mutex free because the mutex can cause a deadlock at the very beginning.

Do you want me to remove the call and fix the tests too?

[gimait]

I don't think you can simply remove it. If we want to remove it from here, we will need to do some other changes. There might be several places where the main loop might attempt to make use of the first map when was received, but not yet processed. We need to ensure that all places where the processed map is used are aware of the possibility of the map not being yet initialised.

Then, in my opinion, it is good that that test fails, it should, because you are changing the behaviour of this plugin. However, the tests failing are not a reason to not do the full change to processing in the main loop. If we want to move that last call to processMap, then we should fix these tests and/or write new ones that fit the current implementation.

So I needed to keep this call here and keep it mutex free because the mutex can cause a deadlock at the very beginning.

I understand that you are focused on fixing the deadlock, but that doesn't mean you can simply leave unprotected code behind.
There are three points I want to do here, which I'm not sure have been considered yet:

First, in line 225 of the static layer we are locking the same mutex. This means you might not have fixed the lock entirely, only you don't see it anymore because it only happens with subsequent calls to incomingMap.

Second, all this code is retrieving data and performing operations on the layered costmap without protection, which to me, sounds like a very bad idea.

Lastly, setting map_received_ without protection might bring undesired behaviour when checking it from the main loop. For example, in the main thread, you could have just called updateBounds with map_received_ set to false, followed by a updateCosts with map_received_ set to false.

As a rule of thumb, never leave a variable shared between threads unprotected. I am sorry, I know I am being a bit bipolar about this. I sometimes feel like it's okay to break that rule, but things here look a bit too complex to leave the code unprotected. I think I wasn't thorough enough about the threading issues before my previous review, I apologize.

---

To sum up, you should move back the lock to the beginning of this method. I don't think you will see the deadlock if you do so. Then it's up to you to remove the last processMap call here and fix the tests.

Any thoughts @SteveMacenski? I think you know better if it's best to remove that last call to processMap or keep it.

[SteveMacenski]

I don't quite understand the issue. In steady state, processMap is only called in the updateBounds function, so we're really only talking about the very first map when the current costmap is not set to any particular valid size/content.

In the situation we're talking about with the first map, the map_received_ boolean at the start of both Update functions will make them immediately return without trying to do anything (if false) - which is before any map is processed.

So, if no map is processed before updateBounds its OK. If its processed before that point in the map callback, also OK.

If no map is processed before updateCosts and updateBounds, OK. If its processed after updateBounds but before updateCosts, that's where it could potentially be not OK. I think my comment just below lays out a solution to that (maintain the state of the "updatedness" from updateBounds to also use in updateCosts).

But in terms of leaving things unprotected, this seems like it does that fine? Because of the special case that map_received_ immediately returns the functions without doing anything.

[SteveMacenski]

From #1625

The problem is that the dimension of the costmaps should not change between the updateBounds and the updateCosts calls in the updateMap function of the LayeredCostmap.

Now the updates are sequential in updateBounds so that should work fine. The exception is that first map coming in which I suppose could cause a problem in odd situations where map_received_ is set to true so in updateCosts, its not passed through.

I suppose we could re-introduce that atomic bool for if there's processing currently happening, though I don't love that logic bleeding into all of the functions - both updates + the callback.

Another option would be to introduce a new class member boolean map_received_in_update_bounds_ which is set to true/false based on the value of map_updated_ in updateBounds so that we use the global state for the first update function, and then use whatever status was there for updateCosts instead of the global state. That variable would never touch the map callback thread so it doesn't need a mutex or be atomic. That would deal with the 1st map problem without blocking the threads anymore than necessary.

Or maybe even has_updated_data_ would do that already if that's set to true?

[daisukes]

@gimait, @SteveMacenski, Thank you for the comments!

Let me summarize based on my understanding.

• The current main code has two issues; (1) deadlock in incomingMap and (2) map_received_ can be changed between updateBounds and updateCosts at the first incomingMap call
• My PR focused on issue (1) and does not fix issue (2)
• To fix issue (2), introducing map_received_in_update_bounds_ would be good

Then it’s up to you to remove the last processMap call here and fix the tests.
I like removing the last processMap call in incomingMap to make it as simple as possible, but as @gimait mentioned, it will change the behavior of the system.

How about

1. I will make a change to fix (2) in this PR
2. I will also make another PR that will be a version having no processMap in incomingMap and fixed tests

and then, you will decide on which version you will merge. What do you think?

[SteveMacenski]

LGTM - can you confirm the deadlock is gone still (just sanity checking)?

Are some tests still failing for static layer due to these changes? That absolutely needs to be fixed before merging this PR. Why remove processMap in incomingMap? I thought we found the solution to that via map_received_in_update_bounds_.

Can you rebase / pull in main? There are some test failures that should be green b/c its outdated. I want to see CI green on all PRs from now on :-)

[daisukes]

I've merged the latest main branch and checked the deadlock is gone with my test code (15 mins).

[codecov]

Codecov Report

Merging #3145 (6cd3a4f) into main (7813fee) will decrease coverage by 0.19%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##             main    #3145      +/-   ##
==========================================
- Coverage   83.04%   82.85%   -0.20%     
==========================================
  Files         339      339              
  Lines       15233    15233              
==========================================
- Hits        12650    12621      -29     
- Misses       2583     2612      +29     

Impacted Files	Coverage Δ
...ostmap_2d/include/nav2_costmap_2d/static_layer.hpp	0.00% <ø> (ø)
nav2_costmap_2d/plugins/static_layer.cpp	70.68% <100.00%> (-4.19%)	⬇️
...av2_dwb_controller/dwb_critics/src/oscillation.cpp	72.00% <0.00%> (-24.00%)	⬇️
...ostmap_2d/plugins/costmap_filters/speed_filter.cpp	94.49% <0.00%> (-1.84%)	⬇️
nav2_lifecycle_manager/src/lifecycle_manager.cpp	91.98% <0.00%> (-0.95%)	⬇️
nav2_amcl/src/amcl_node.cpp	71.79% <0.00%> (+0.14%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

[SteveMacenski]

In your view, is this ready to merge or are there some updates to tests you want to make?

[daisukes]

I think it is ready.

[SteveMacenski]

Awesome, thanks for helping identify and fix the issue!