Add 'update costmap on request' option to costmaps

[SteveMacenski]

Via https://github.com/ros-planning/navigation2/issues/3091 it seems like a sensible option to allow users to update the costmap only on a request (e.g. planning or control). This comes with downsides I outline in my response in #3091 but there are definitely situations where that would be a sensible choice.

This should be really easy as the user outlines in #3091 + adding a parameter to toggle (or if the update rate is set to 0.0?).

@cf-zhang would that be something you'd be open to contributing? That + the suggestions in the other ticket would put you back in line with Nav2 source and you wouldn't have to maintain a fork.

[enesbedir1]

hİ @SteveMacenski, I want to give a try to this one if @cf-zhang does not interest it or I can help him.

I also take a look to #3091 and #3091 focuses to "Recovery server should contain a costmap". As I understand, the goal of the task is changed to current one, right?

This is what I understand from the task: We want to set the update_frequency(https://github.com/ros-planning/navigation2/blob/634d2e3d9b6bde5558c90fe6583d52b3ed66cf55/nav2_bringup/params/nav2_params.yaml#L173) to zero. Then we want to update a local_costmap or global_costmap just with a request.

[SteveMacenski]

Correct - this is only to add the option to update only upon an algorithm request and not instead by a regular update thread.

So there should be a new parameter update_on_request (or something) which would then not set up the thread for updating the costmap at a fixed rate and instead we only update it when a new request comes in for an algorithm computation. We'd just ignore the update_frequency in this case.

[cf-zhang]

Correct - this is only to add the option to update only upon an algorithm request and not instead by a regular update thread.

So there should be a new parameter update_on_request (or something) which would then not set up the thread for updating the costmap at a fixed rate and instead we only update it when a new request comes in for an algorithm computation. We'd just ignore the update_frequency in this case.

Navigation2 already supports this option, nothing has been changed except one line of code. I dealed with this problem as follows:

• set the parameter update_frequency to 0.0
• add initialized_ = true; before start(); in the function Costmap2DROS::on_activate() of costmap_2d_ros.cpp https://github.com/ros-planning/navigation2/blob/664c09f6d4dfd769592b2611a1461c9acf26b1f8/nav2_costmap_2d/src/costmap_2d_ros.cpp#L250
• use updateMap() when I need the latest costmap2d, so I modify waitForCostmap() in planner or controller server like this.

replace

void PlannerServer::waitForCostmap()
{
  // Don't compute a plan until costmap is valid (after clear costmap)
  rclcpp::Rate r(100);
  while (!costmap_ros_->isCurrent()) {
    r.sleep();
  }
}

with

void PlannerServer::waitForCostmap() {
  // Don't compute a plan until costmap is valid (after clear costmap)
  costmap_ros_->updateMap();
}

Then it worked the way I wanted.

[SteveMacenski]

Very sensible. I'd like to have a separate parameter for update_on_request just in case there's some really odd usecase (or more likely, benchmarking / test case) where they just want the costmap updated 1 time and never again with 0.0 frequency. That way, also, that boolean value can be grabbed by the planner/controller servers to know which of the waitForCostmap behaviors to use (as well as if to set initialized_ before start()).

But overall, this is a pretty simple change, I'll add it to the queue of good tasks for new contributors! Unless you want to submit a PR :wink:. I'll get around to it eventually myself either way, but its low-priority.

[gros-antoine]

It seems that no one is working on this? If so, I can try my hand at it. To be clear, do I just need to add a new parameter like update_on_request that would not set up the updating and publishing thread of the costmap? Or do I have to also implement a new function to update and publish the costmap to replace what is done in the previously mentioned thread?

[SteveMacenski]

Yeah, add a parameter, don't create the thread to update the costmap, and when a request comes in on the servers, trigger the updateMap then and publish it. It might makes sense to break out the mapUpdateLoop function into a few smaller ones that can be used in both locations.

[JohnTGZ]

@gros-antoine Hi, are you still working on this? I have some bandwidth and would like to tackle this as a first issue :)

[JohnTGZ]

Hi @SteveMacenski, I have made a PR for this feature #3297, open to suggestions and criticism! Also, should I be working off the main branch instead of humble? And would it be necessary to add any tests in this case?

[SteveMacenski]

Yes! Main would be the place that PR should be targeted since its a significant behavioral change. That way its available in future distributions.

A test would be fantastic!