Update costmap on request by JohnTGZ · Pull Request #3300 · ros-navigation/navigation2

JohnTGZ · 2022-11-26T08:07:01Z

Basic Info

This PR targets #3121, implement the 'update costmap on request' function via the boolean parameter update_on_request which can be specified on the costmap layer.
This PR is also a continuation from #3297 , which is closed as it was supposed to be merged into the main branch.

Info	Please fill out this column
Ticket(s) this addresses	(#3121)
Primary OS tested on	(Ubuntu 22.04)
Robotic platform tested on	(turtlebot gazebo simulation, rolling distro)

Description of contribution in a few bullet points

Implemented update_on_request parameter for the costmap server
- If True, the thread running the map update loop is not initialized or is terminated. The planner and controller server will then have to manually call updateAndPublishMap() on each update loop.
- If False, the thread running the map update loop will be initialized, which is the default behaviour.
- Param can be dynamically reconfigured during runtime.
The planner server and controller server will use the getter function costmap_ros_->isUpdateOnRequest() to determine if it will need to call updateAndPublishMap().
Move starting and stopping of thread into separate routines mapUpdateThreadOff() and mapUpdateThreadOn() to make it easier to manage the map update loop thread.

Description of documentation updates required from your changes

Add update_on_request parameter to the costmap configuration documentation for navigation.ros.org

Future work that may be required in bullet points

Document the use case and implications of the update_on_request parameter.

Possible use-cases: Benchmarking, test cases.
Possible implications: As mentioned in Feature Request（galactic）: RecoveryServer use it's own costmap for collision check #3091 .Missing out on sensor information if the planner or controller update frequency is low enough to result in long intervals between requests for costmap updates. Or if the planner/controller update frequency is too high, the costmap update rate will be unnecessarily high, and take up extra CPU resources.

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

Instructions for testing

# Launch the turtlebot simulation world 
ros2 launch nav2_bringup tb3_simulation_launch.py

# Modify parameters during run-time
ros2 param set /local_costmap/local_costmap update_on_request <BOOLEAN_VAL>

…ng of code review comments

Signed-off-by: johntgz <johntan@openrobotics.org>

…tion2 into update-costmap-on-request

… use-case Signed-off-by: johntgz <johntan@openrobotics.org>

mergify · 2022-11-26T08:07:35Z

@JohnTGZ, 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

JohnTGZ · 2022-11-26T08:19:37Z

@AlexeyMerzlyakov I just realised that if we are to combine the updating and publishing of map into the updateAndPublishMap() method, then if update_on_request is TRUE, the publish_frequency will not matter.
If it is important for the costmap server to still publish with a fixed frequency, perhaps we can break the publishing out into another thread, what do you think?

JohnTGZ · 2022-11-26T08:27:09Z

please properly fill in PR template in the future

I wonder what I might have missed out here?

AlexeyMerzlyakov

Okay, good. Please check the review items for this PR and next step required - is to add a new testcase on this feature.
I think, this should be:

New unit test for the update-on-request feature. From brief view, it could be a Costmap2DROS class object with prepared layered_costmap_ inside tuned on one plugin, e.g. on static_layer. Static layer could listen the incoming OccupancyGrid map published by simple OccupandyGrid publisher (also added inside the test). You could publish some occupancy grid once, then run Costmap2DROS until the costmap is not being published (and be equal to initial OccupancyGrid). Then update the incoming map, manually trigger updateAndPublishMap() and check that costmap was updated correctly. If necessary, the test might contain Costmap2DROS class wrapper, inherited from base class that is you will have an access to all protected parent class fields. Also, I think, we need to check that if update_on_request is set to false, costmap is being updated automatically.
But in overall, it could be something more simpler, if you could find better way and propose instead.
Supporting "update_on_request" parameter in dynamic params test test/integration/dyn_params_tests.cpp

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

nav2_costmap_2d/src/costmap_2d_ros.cpp

SteveMacenski

Generally speaking, this looks good to me, but heed @AlexeyMerzlyakov's comments. I'll take a look once he fully approves, but all looks good to me from a first glance.

You do have some linting issues, which CI should point out to you (or ament_cpplint / ament_uncrustify)

I just realised that if we are to combine the updating and publishing of map into the updateAndPublishMap() method, then if update_on_request is TRUE, the publish_frequency will not matter.

Is that true? Its still being used in the updateAndPublishMap() function to only publish after the duration is expired from the last publication. Isn't the publish_cycle_ still being respected? It may be not queried at the regular looping rate anymore, but should at least enforce approximate publication -- or at least not any more frequently than that.

If it is important for the costmap server to still publish with a fixed frequency, perhaps we can break the publishing out into another thread

I don't think its very important to be exactly at a fixed rate, its more about throttling than it is regularity. If you have updates to the local costmap at 20hz (which is not terribly uncommon), publishing that at 20hz is very expensive just for visualization. 5hz or something is very reasonable, instead.

I wonder what I might have missed out here?

Nothing, the macro is broken and I haven't had a time to fix it yet. Mergify keeps working for a few weeks, then breaks 🙄

On unit test: you could simply create a costmap with this parameter on with a static layer. Send it a random static map (full obstacle, a smiley face, whatever). Check that the costmap is still empty. Then trigger that update method and check that there are new values -- but on that update, not due to a regular background thread. I think that's also what Alexey described, more or less.

mergify · 2022-12-06T18:45:09Z

This pull request is in conflict. Could you fix it @JohnTGZ?

SteveMacenski · 2022-12-06T22:17:52Z

@AlexeyMerzlyakov please rereview

JohnTGZ · 2022-12-07T04:05:46Z

Hi all, sorry that I have not had time to add the test yet, I would be able to do it by friday

AlexeyMerzlyakov

A few strokes remained:

Please correct the logic of updateAndPublish() call in Costmap2DROS::on_activate()
Rebase onto latest main branch and correct the patch accordingly

And unit test is missing for this feature. You can take nav2_costmap_2d/test/regression/plugin_api_order.cpp as a basis: this testcase creates a new Costmap2DROS object and sets a plugin (in your case it will be static_layer) for it, then executes.

nav2_costmap_2d/src/costmap_2d_ros.cpp

AlexeyMerzlyakov · 2022-12-07T04:04:48Z

nav2_costmap_2d/src/costmap_2d_ros.cpp

 }

+void
+Costmap2DROS::updateAndPublishMap()


Please rebase onto to the latest main branch: there is a new indicator variable and behavior used to verificate that Costmap2DROS was started. It ought to be added to updateAndPublishMap(), since this API logic should be called only of Costmap2DROS was already started:

void Costmap2DROS::updateAndPublishMap() { if (stopped_) { // Do not execute if start() did not complete plugins activation return; } nav2_util::ExecutionTimer timer; // Measure the execution time of the updateMap method timer.start(); updateMap(); timer.end(); ... }

… least once when update_on_Request parameter is true Signed-off-by: johntgz <johntan@openrobotics.org>

Signed-off-by: johntgz <johntan@openrobotics.org>

JohnTGZ · 2022-12-17T08:33:00Z

@AlexeyMerzlyakov Thank you for the guidance and suggestions, I have made the suggested changes and added a unit test as well, please take a look when you are free :)
Also, I think I made mistake by merging with main, instead of rebasing it, let me know if you need me to rectify this.

JohnTGZ · 2022-12-17T08:39:25Z

nav2_costmap_2d/test/unit/costmap_update_on_request_test.cpp

+
+  void waitForMap()
+  {
+    while (!this->isCurrent()) {


One thing that I can't quite figure out is if we still need to check if the layered costmap is current? From the static layer plugin, it is set to true after receiving a new map. I can remove this check if that is the case and just do a normal spin.

I do not think this is a problem: another integration tests are also uses the same approach. As indicator current_ is also OK: this means that we could use the map (for static layer in this case - it was received).

AlexeyMerzlyakov · 2022-12-20T14:35:15Z

nav2_costmap_2d/src/costmap_2d_ros.cpp

+  // Execute after start() will complete plugins activation
+  if (!stopped_) {
+    // Measure the execution time of the updateMap method


Since it have already begun to do, why not optimize some: instead of do all this stuff inside if-statement, it is better to add something like mentioned in previous comment item:

if (stopped_) { // Do not execute if start() did not complete plugins activation return; } nav2_util::ExecutionTimer timer; // Measure the execution time of the updateMap method timer.start(); updateMap(); timer.end(); ...

Which is more readable instead of having one more indent in the code. What do you think?

Yes, this is definitely more readable, I will make the changes.

AlexeyMerzlyakov · 2022-12-20T14:38:46Z

nav2_costmap_2d/test/unit/costmap_update_on_request_test.cpp

@@ -0,0 +1,257 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia


Please, correct the copyright message to one related to you.

AlexeyMerzlyakov · 2022-12-20T14:43:45Z

nav2_costmap_2d/test/unit/costmap_update_on_request_test.cpp

+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav2_costmap_2d/cost_values.hpp>
+#include <nav2_costmap_2d/costmap_2d_ros.hpp>
+#include <nav2_util/occ_grid_values.hpp>


Please use " to address these headers as relative to the ROS/project rather than system ones

AlexeyMerzlyakov · 2022-12-20T15:51:02Z

nav2_costmap_2d/test/unit/costmap_update_on_request_test.cpp

+
+  void waitForMap()
+  {
+    while (!this->isCurrent()) {


I do not think this is a problem: another integration tests are also uses the same approach. As indicator current_ is also OK: this means that we could use the map (for static layer in this case - it was received).

AlexeyMerzlyakov · 2022-12-20T15:58:55Z

nav2_costmap_2d/test/unit/costmap_update_on_request_test.cpp

+
+  // Spin to check that background thread does not update the costmap
+  waitSome(200ms);
+  // rclcpp::spin_some(costmap_ros_wrapper_->get_node_base_interface());


Please, remove unnecessary commented line

AlexeyMerzlyakov · 2022-12-20T15:59:53Z

nav2_costmap_2d/test/unit/costmap_update_on_request_test.cpp

+  publishOccupancyGrid();
+
+  // Spin until the background thread updates the map
+  waitSome(200ms);


Since there is the waitForMap() call on next line, do we really need this wait?

The issue is that without waiting for the costmap update thread to spin for at least 1/map_update_frequency, the map will not be updated by the costmap node.

void Costmap2DROS::mapUpdateLoop(double frequency) { // --snip-- rclcpp::WallRate r(frequency); // 200ms by default while (rclcpp::ok() && !map_update_thread_shutdown_) { updateAndPublishMap(); // Make sure to sleep for the remainder of our cycle time r.sleep(); // --snip-- }

AlexeyMerzlyakov · 2022-12-20T16:09:27Z

Also, I think I made mistake by merging with main, instead of rebasing it, let me know if you need me to rectify this.

I am not sure, how GitHub will handle correctly non-linear merges. So, it is better to force-push your branch made over latest main.

If I did this, I would like to rename my_branch to something else, like my_branch_old. Then check out to the main, update it to the latest vanilla state, then create my_branch again (it actually will be a new branch), and apply all your changes (e.g. using git cherry-pick or manually making the patch) from my_branch_old to my_branch. Force-push should lead your local GitHub repository to "forget" about old non-linear history.

You also could verify the results by clonning https://github.com/JohnTGZ/navigation2.git in a /tmp directory after it, and then using git lg1 (you could search this alias through the Internet). Hope this will help

johntgz added 7 commits November 25, 2022 11:37

Implementation of update_on_request for costmap layers, with addressi…

18c3698

…ng of code review comments

change name of function 'updatePublishMap' to 'updateAndPublishMap'

8ca27d2

use routines for starting and stopping thread for map update loop

88ebd8d

use routines for starting and stopping thread for map update loop

7c82a2a

Signed-off-by: johntgz <johntan@openrobotics.org>

Merge branch 'update-costmap-on-request' of github.com:JohnTGZ/naviga…

90721cc

…tion2 into update-costmap-on-request

linting

7836f99

remove update_on_request param from param file, as it is not a common…

89b85ee

… use-case Signed-off-by: johntgz <johntan@openrobotics.org>

JohnTGZ mentioned this pull request Nov 26, 2022

implemented 'update costmap on request' option #3297

Closed

7 tasks

JohnTGZ mentioned this pull request Nov 26, 2022

added documentation for 'update_on_request' parameter ros-navigation/docs.nav2.org#376

Closed

AlexeyMerzlyakov reviewed Nov 28, 2022

View reviewed changes

SteveMacenski reviewed Nov 28, 2022

View reviewed changes

addressed code review comments

94bad9a

AlexeyMerzlyakov reviewed Dec 7, 2022

View reviewed changes

JohnTGZ and others added 3 commits December 15, 2022 22:12

Merge branch 'main' into update-costmap-on-request

26ab7f0

added conditional to ensure that the updateAndPublishMap is called at…

67f078f

… least once when update_on_Request parameter is true Signed-off-by: johntgz <johntan@openrobotics.org>

added unit test for update_on_request parameter

90981a9

Signed-off-by: johntgz <johntan@openrobotics.org>

JohnTGZ requested a review from AlexeyMerzlyakov December 17, 2022 08:32

JohnTGZ commented Dec 17, 2022

View reviewed changes

AlexeyMerzlyakov reviewed Dec 20, 2022

View reviewed changes

addressed code review comments and reorganized unit test

3034390

JohnTGZ closed this Dec 22, 2022

JohnTGZ deleted the update-costmap-on-request branch December 22, 2022 05:40

		@@ -0,0 +1,257 @@
		// Copyright (c) 2022 Samsung R&D Institute Russia

Conversation

JohnTGZ commented Nov 26, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

Instructions for testing

Uh oh!

mergify bot commented Nov 26, 2022

Uh oh!

JohnTGZ commented Nov 26, 2022

Uh oh!

JohnTGZ commented Nov 26, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

AlexeyMerzlyakov left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mergify bot commented Dec 6, 2022

Uh oh!

SteveMacenski commented Dec 6, 2022

Uh oh!

JohnTGZ commented Dec 7, 2022

Uh oh!

AlexeyMerzlyakov left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

AlexeyMerzlyakov Dec 7, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

JohnTGZ commented Dec 17, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

JohnTGZ Dec 22, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

AlexeyMerzlyakov commented Dec 20, 2022

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

JohnTGZ commented Nov 26, 2022 •

edited

Loading

JohnTGZ commented Nov 26, 2022 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

AlexeyMerzlyakov left a comment •

edited

Loading

AlexeyMerzlyakov Dec 7, 2022 •

edited

Loading

JohnTGZ commented Dec 17, 2022 •

edited

Loading

JohnTGZ Dec 22, 2022 •

edited

Loading