Added new costmap buffer to resize map safely #1837
Conversation
Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
|
So I'm not sure that I think this is the right solution. In the case that the global costmap updates slowly, you're creating a huge amount of latency. I think we should reverse the strategy of having a bool on if we have a new map available and replace it with if we're in the middle of a updateBounds -> updateCost. That way it only buffers when we're in that awkward middle area and otherwise we get no additional latency. At the end of updateCost, update if necessary if there's a buffered map around. I'm also debating the mutex bit right now... it would make this cleaner... |
|
You are right, ProcessMap can be moved to the end of UpdateCosts, however, that is a rather small optimization I guess. The problem is that ProcessMap updates the whole layered costmap, so what is needed is not as easy as updating the costmap_ variable.. There is going to be some latency somewhere when updating this. The question is, where should this fall? In the LayeredCostmap thread or in the incomingMap callback thread? None of these solutions seem too clean or nice, but they are the minimal changes, I think. To make it look nice I would look into a bigger refactor. Should I do that? |
|
I'm just trying to make sure that in case the costmap update rate is low, that the map can get updated ASAP when there's no conflict. Right now, if the update rate was once every 10 seconds, it might take 10 seconds to process a new map. We could do that with a mutex or switching what the boolean variable represents |
|
I like that idea, we can have something like this then:
It bothers me a bit that this code is so complex though.. I wish we could simply merge updateBounds and updateCosts, but I'm not sure why should we wait to verify all layers before updating their costs. Wouldn't it be better to run updateBounds and updateCosts in the same loop, calling them one after the other for each layer? Instead we currently call updateBounds in all layers and then all updateCosts, having a nasty return in the middle that makes complex protecting these functions. |
|
I don't think you need the second bool if you play games with the pointer. If the pointer isn't null or something, then set it at the end of the updateCosts function. Just null it back out every time you clear it. The bool for if updating should be an atomic since its being touched by 2 threads. You need to call updateBounds on all first so that as 1 layer expands the boundary, the others that might need to update (like the inflation layer) knows all the space it needs to update. |
Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
|
Sorry for the inactivity! At the end I ended up doing what we agreed in the last comments, please review that everything is correct (I hope I didn't but I might have missed something). I only placed the update map at the beginning of updateBounds because we are not guaranteed to enter UpdateCosts every time, and this way the map can still be updated. There is still the option of adquiring the mutex from the layered costmap in the processMap call, and that might be cleaner, but this should give us a small performance optimization over the mutex. Please let me know your thoughts. |
| map_buffer_ = new_map; | ||
| } else { | ||
| processMap(*new_map); | ||
| map_buffer_ = nullptr; |
There was a problem hiding this comment.
You don't need to set to nullptr here. If you set it, it will be reset below in the update function.
There was a problem hiding this comment.
I think it is necessary. If the incomingMap is called while updating the layer, and then again after the update is finished, the last call should pe processed, and the buffer emptied. Otherwise in this case the new map would be processed twice, once here, another time in the update function.
There was a problem hiding this comment.
If map_buffer_ is set to nullptr to begin with (you should do that in constructor's :), then it is only not nullptr when 259 sets it to something else. In the next function, you process the map and then reset it to nullptr. At that point, I don't think there's any case when this is not nullptr where setting it here is necessary. You have a mutex locked above this so only this callback or one of the functions can be in use at the same time, effectively locking this variable -- unless I'm missing something.
There was a problem hiding this comment.
Yeah, so both functions can call the processMap, therefore if we don't clear the map_buffer_ here, when we get to the update we see that efectively there is something in the buffer and we update it again. I think. The fact of having the lock doesn't change that: When we are here, this function has the lock, and processes the map. Then the update is waiting, and when the lock gets free, it would check the buffer, and call processMap again if it's not empty. I think you are seeing the practical case, but in theory, it would be possible to call incomingMap several times between calls to updateBounds, including during the execution of updateBounds.
But I see how this can be confusing, and tricky to follow. Maybe it's best to simply get the layered_costmap mutex at the beginning of processMap and leave the rest as it was..
I'll initialize the buffer to nullptr also, I missed that one.
There was a problem hiding this comment.
I guess let me game this out without the 262 map_buffer_ = nullptr:
- I start up (so map_buffer_ is nullptr) have a new map, not in the middle of a process cycle so it just directly updates. Do this 10 times, still never set to anything other than nullptr
- I start up (so map_buffer_ is nullptr) have a new map, not in the middle of a process cycle so it just directly updates. Then I update bounds 10 times, so still nullptr so never gets called in the update function
- I start up (so map_buffer_ is nullptr) have a new map, its now in the middle of an update cycle so it stores the map in
map_buffer_between the 2 functions after the mutex is released and then in the updateCosts it uses the old map since there's no processMap() call in updateCosts. Then we wait for the next iteration of the costmap to add / process the map - I start up (so map_buffer_ is nullptr) have a new map, its now in the middle of an update cycle so it stores the map in
map_buffer_between the 2 functions after the mutex is released and then in the updateCosts it uses the old map since there's no processMap() call in updateCosts. Then we have a second new map before the updateBounds can be called again. If we're not in an update cycle, then that new map will be directly processed but the old map will still be stored and processed next round.
You are correct, we need to set to reset in case there's something there. Thanks!
There was a problem hiding this comment.
Just make the other changes and we should be GTG
There was a problem hiding this comment.
Okay I'll push the update during the day.
Just to clarify why I placed the processMap at the beginning of updateBounds and not at the end of updateCosts:
The line 168 of layered_costmap.cpp can prevent the updateCosts call from being done, which would lock the costmap if the reason for the error comes from the static_layer's map (if that would be the case, the logic would prevent the static map from being updated). By placing the processMap call at the beginning of updateBounds, we ensure that the map would be updated every time there is a new map, without any real changes on the performance of the layered costmap.
There was a problem hiding this comment.
Okay, I'll fix the branch and the other changes and push during the day.
Just to clarify why I placed the call to processMap at the beginning of updateBounds and not at the end of updateCosts:
The line 168 of layered_costmap.cpp can prevent the call of updateCosts. This is problematic when the reason why we end up returning there is related to the static layer, because in that case, the incomingMap call would be prevented from updating the map (the 'update_in_progress' flag is set to true until the end of updateCosts).
It might seem like a lot of conditions should be met for that to happen, but it could happen. By moving the processMap to the beginning of updateBounds, we prevent this logic lock from happening without any performance penalty.
There was a problem hiding this comment.
Beginning makes sense, no issue
|
@gimait please rebase / pull in master to let CI build |
|
This should work, but would double buffering costmap_ not be a better
solution. Then you can process the map in incomingMap and only have to swap
pointers at the beginning of updateBounds?
…On Thu, Jul 23, 2020 at 3:09 AM Steve Macenski ***@***.***> wrote:
***@***.**** commented on this pull request.
------------------------------
In nav2_costmap_2d/plugins/static_layer.cpp
<#1837 (comment)>
:
> if (!map_received_) {
map_received_ = true;
+ processMap(*new_map);
+ }
+ if (processing_layer_.load()) {
+ map_buffer_ = new_map;
+ } else {
+ processMap(*new_map);
+ map_buffer_ = nullptr;
Beginning makes sense, no issue
—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
<#1837 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/APCLV467OB7ESL5EZFGHEXLR46EUZANCNFSM4OM5PW2Q>
.
|
Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
The only problem is that the processMap function updates the whole layered_costmap, not only the local one.. then it is not really so easy to swap the pointers. Please test this with your code, since you were able to reproduce it rather easily it would be a lot of help if you can run it and see if it actually works or some other issues appear. |
Codecov Report
@@ Coverage Diff @@
## master #1837 +/- ##
==========================================
+ Coverage 68.82% 69.42% +0.60%
==========================================
Files 218 218
Lines 10600 10614 +14
==========================================
+ Hits 7295 7369 +74
+ Misses 3305 3245 -60
Continue to review full report at Codecov.
|
|
You're right it was too early in the morning for me ;) But as you said the
layered_costmap is locked anyway in processMap, so there is actually no
speed advantage at all, only more added complexity to the code.
Not to be a party pooper, i've tested your solution and it works.
But if you don't mind I will stick to my own.
…On Thu, Jul 23, 2020 at 1:02 PM Aitor Miguel Blanco < ***@***.***> wrote:
Then you can process the map in incomingMap and only have to swap pointers
at the beginning of updateBounds?
The only problem is that the processMap function updates the whole
layered_costmap, not only the local one.. then it is not really so easy to
swap the pointers.
Please test this with your code, since you were able to reproduce it
rather easily it would be a lot of help if you can run it and see if it
actually works or some other issues appear.
—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
<#1837 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/APCLV4ZSAKUHU7SUSGAWSK3R5AKFTANCNFSM4OM5PW2Q>
.
|
|
@dwisse you're welcome to do that - but I would not recommend it. Messing with the higher level locks will probably increase your CPU usage from more mutex thrashing & certain costmap layers might not be compatible with that method. This is supposed to be a distributed solution to enable multithreading if you liked. @gimait anything else you want to update? |
|
Can you be more specific?, As the layered costmap is locked is anyway used
in processMap I don;t see any multithreading advantage, If you're so
certain about the increase in CPU usage, can you point out exactly where,
maybe I can learn something
…On Thu, Jul 23, 2020 at 9:00 PM Steve Macenski ***@***.***> wrote:
@dwisse <https://github.com/dwisse> you're welcome to do that - but I
would not recommend it. Messing with the higher level locks will probably
increase your CPU usage from more mutex thrashing & certain costmap layers
might not be compatible with that method. This is supposed to be a
distributed solution to enable multithreading if you liked.
@gimait <https://github.com/gimait> anything else you want to update?
—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
<#1837 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/APCLV4YNTU7CDXC5SKHO553R5CCE7ANCNFSM4OM5PW2Q>
.
|
|
You have the layer locking the entire environmental representation. There are other services and users of this costmap that need access on their own schedule, not on the schedule of the costmap update thread you're blocking. For instance, if my planner was called, your solution is blocking the controller or planner from doing its job during any update in this way. Overall, it breaks many encapsulation concepts / implementations used in costmap_2d. |
|
But when you call process incomingMap in updateBound the same thing
happens, you also lock the entire environmental representation, and you
will also influence the timing of the controller or planner in the same way
…On Thu, Jul 23, 2020 at 9:16 PM Steve Macenski ***@***.***> wrote:
You have the layer locking the entire environmental representation. There
are other services and users of this costmap that need access on their own
schedule, not on the schedule of the costmap update thread you're blocking.
For instance, if my planner was called, your solution is blocking the
controller or planner from doing its job during any update in this way.
—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
<#1837 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/APCLV476GB65XZY2S6AHSWLR5CEBNANCNFSM4OM5PW2Q>
.
|
|
No, you lock the static map update, which isn't required for the navigation system to access the layered costmap. |
|
Sorry i don't understand your last email. I don't want to be difficult but
it would be helpful if you could sketch a concrete example to point out the
difference.
…On Thu, Jul 23, 2020 at 9:47 PM Steve Macenski ***@***.***> wrote:
No, you lock the static map update, which isn't required for the
navigation system to access the layered costmap.
—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
<#1837 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/APCLV46RQ4LBJIT3X326JLDR5CHWNANCNFSM4OM5PW2Q>
.
|
|
I'd recommend using this as we've designed. |
|
@dwisse I'm not sure we are talking about the same mutex. Each costmap has a mutex to protect it during updates. This means that both the layered_costmap and static_layer have their own mutex. Coming from that base, the layered costmap is only locked at line 176 and released right after. In #1625 you had a problem when using a rolling map, which was not protected because the layered_costmap's mutex is not locked in that case. What I tried to do in this PR was to add protection for that case without stopping the whole layered costmap. I believe there should be an advantage to this approach when several plugins (i.e., layers) are in use, since we include the opportunity for the map to be updated at the same time as other plugins might be processed, but please let me know if I missed something. If you have other suggestions or you think this can be improved, I would like to encourage you to submit a PR with what you would like to see working. I personally think it is good to share your thoughts and contribute with the community: you can get a lot of feedback and help, and you can also help others that might have similar problems to yours, and the only extra thing you would have to do is create the PR, maybe a ticket. |
* Added new costmap buffer to resize map safely Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Update map if the layer is not being processed. Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Updated bool name and added buffer initialization Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
* Add slam toolbox as exec dep for nav2_bringup (#1827) * Add slam toolbox as exec dep * Added slam toolbox * Changed version of slam toolbox to foxy-devel * Added modifications to allow for exec depend of slam toolbox without breaking docker / CI * reformatted skip keys * Added tabs to skip key arguments * Removed commented out code block * Add unit tests for follow path, compute path to pose, and navigate to pose BT action nodes (#1844) * Add compute path to pose unit tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Add follow path action unit tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Add navigate to pose action unit tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * adding foxy build icons to readme (#1853) * adding foxy build icons * adding dividers * sync master from foxy version updates (#1852) * sync master from foxy version updates * syncing foxy and master * Add unit tests for clear entire costmap and reinitialize global localization BT service nodes (#1845) * Add clear entire costmap service unit tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Add reinitialize global localization service unit test Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Temporarily disable dump_params test (#1856) Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * commenting out unused validPointPotential (#1854) * Adding ROS2 versions to issue template (#1861) * reload behavior tree before creating RosTopicLogger to prevent nullptr error or no /behavior_tree_log problem (#1849) Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * Add citation for IROS paper (#1867) * Add citation We'll want to add the DOI once published * Bump citation section above build status * Fix line breaks * Changes RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED to RCUTILS_LOGGING_BUFFERED_STREAM (#1868) * Added parameters to configure amcl laser scan topic (#1870) * Added parameters to configure amcl topic * changed parameter to scan_topic and added to all the configuration files * added scan_topic amcl param to docs * Satisfied linter * Move dwb goal/progress checker plugins to nav2_controller (#1857) * Move dwb goal/progress checker plugins to nav2_controller Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Move goal/progress checker plugins to nav2_controller Address review comments Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Move goal/progress checker plugins to nav2_controller Use new plugin declaration format and doc update Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Update bringup.yaml for new plugins in nav2_controller Also remove redundent file from dwb_plugins Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Fix doc errors and update remaining yaml files Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Remove mention of goal_checker from dwb docs Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Add .plugin params to doc Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Tests for progress_checker plugin declare .plugin only if not declared before Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Tweak plugin names/description in doc Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Follow pose (#1859) * Truncate path finished Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Follow Pose finished Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Change names. Add test and doc Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Change names and check atan2 values Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Checking Inf/NaNs and trucate path changes Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Revert changes in launcher Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Documenting Tree Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change TruncatePath from decorator to action node Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Remove Deprecated Declaration (#1884) * Remove deprecated rclcpp::executor::FutureReturnCode::SUCCESS in favor of rclcpp::FutureReturnCode::SUCCESS Signed-off-by: Hunter L. Allen <hunter.allen@ghostrobotics.io> * Update nav2_util/include/nav2_util/service_client.hpp Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com> Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Added new costmap buffer to resize map safely (#1837) * Added new costmap buffer to resize map safely Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Update map if the layer is not being processed. Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Updated bool name and added buffer initialization Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Fix dump params tests (#1886) Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Move definitions in tf_help to separate src cpp file (#1890) * Move definitions of functions in tf_help to separate src cpp file to avoid multiple definition error * Fix linting issues * Make uncrustify happier * More format fixing * resolved the simulated motion into its x & y components (#1887) Signed-off-by: Marwan Taher <marokhaled99@gmail.com> * Fix nav2_bt_navigator cleanup (#1901) Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Adding some more test coverage (#1903) * more tests * removing old cmake * remove on_errors in specific servers in favor of a general lifecycle warning (#1904) * remove on_errors in favor of a general lifecycle warning * adding removed thing * simply nodes to remove lines (#1907) * Bunches of random new tests (#1909) * trying to get A* to work again * make flake 8 happy * adding cancel and preempt test * planner tests use A* * adding A* note * test with topic * Adding failure to navigate test (#1912) * failures tests * adding copyrights * cancel test in recoveries * Costmap lock while copying data in navfn planner (#1913) * acquire the costmap lock while copying data in navfn planner Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * add costmap lock to dwb local plannger Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * Added map_topic parameters to static layer and amcl (#1910) * see if spin some in cancel will allow result callback (#1914) * Adding near-complete voxel grid test coverage and more to controller server (#1915) * remove erraneous handling done by prior * adding a bunch of voxel unit tests * retrigger * adding waypoint follower failure test, voxel layer integration tests, etc (#1916) * adding waypoint follower failure test * adding voxel, more logging * reset -> clear * minor changes to lower redundent warnings (#1918) * Costmap plugins declare if not declared for reset capabilities (#1919) * fixing #1917 on declare if not declared * fix API * adding CLI test (#1920) * More coverage in map server tests (#1921) * adding CLI test * adding a bunch of new coverages for map_server * Add in range sensor costmap layer (#1888) * range costmap building * range sensor layer working * nav2 costmap pass linter and uncrustify tests * Added back semicolon to range class * Added docs * Added angles dependency * Added BSD license * Added BSD license * Made functions inline * revmoed get_clock * added input_sensor_type to docs * Made defualt topic name empty to cause error * using chorno literal to denote time units * Added small initial test * Fixed linter error, line breaks, enum, logger level, and transform_tolerance issue * fixed segmentation fault in test and added transfrom tolerances to tf_->transform * Got test to pass * Added more tests * removed incorrect parameter declaration * Improved marking while moving tests and added clearing tests * removed general clearing test * changed testing helper import in test * [Test sprint] push nav2_map_server over 90% (#1922) * adding CLI test * adding tests for more lines to map_server * fix last test * adding out of bounds and higher bounds checks * adding planner tests for plugins working * add cleanup * working * ping * [testing sprint] final test coverage and debug logging in planner/controller servers (#1924) * adding CLI test * adding tests for more lines to map_server * fix last test * adding out of bounds and higher bounds checks * adding planner tests for plugins working * add cleanup * working * ping * adding more testing and debug info messages * [testing sprint] remove dead code not used in 10 years from navfn (#1926) * remove dead code not used in 10 years from navfn * ping * inverting period to rate * removing debug statement that could never be triggered by a single non-looping action server * type change * adding removed files * bump to 0.4.2 * add another missing file * add missing files * remove some tests * lint * removing nav2_bringup from binaries Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com> Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com> Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Co-authored-by: Ruffin <roxfoxpox@gmail.com> Co-authored-by: Siddarth Gore <siddarth.gore@gmail.com> Co-authored-by: Francisco Martín Rico <fmrico@gmail.com> Co-authored-by: Hunter L. Allen <hunter.allen@ghostrobotics.io> Co-authored-by: Aitor Miguel Blanco <aitormibl@gmail.com> Co-authored-by: Joe Smallman <ijsmallman@gmail.com> Co-authored-by: Marwan Taher <marokhaled99@gmail.com>
* Added new costmap buffer to resize map safely Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Update map if the layer is not being processed. Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Updated bool name and added buffer initialization Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
* Added new costmap buffer to resize map safely Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Update map if the layer is not being processed. Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Updated bool name and added buffer initialization Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
Signed-off-by: Aitor Miguel Blanco aitormibl@gmail.com
Basic Info
Description of contribution in a few bullet points
This changes still need to be tested, and might need some updates. Please let me know if you see any bugs I might have missed, or if you have any design recommendations.
costmap_variable as well.Future work that may be required in bullet points