Costmap update in accordance to ROS time #3404

AlexeyMerzlyakov · 2023-02-10T13:25:48Z

This is continue of #3327 ticket, supplemented by review items resolved.

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3325
Primary OS tested on	Ubuntu 20.04 with ROS2 Rolling built from sources
Robotic platform tested on	TB3 Gazebo simulation; `colcon test --packages-select nav2_costmap_2d nav2_system_tests`

Description of contribution in a few bullet points

Changed update map frequency control mechanism from WallRate to ROS Timer, so now it respects simulation time if use_sim_time parameters is true.
Calling resume() after stop(): the behavior was changed. In default case, we will hang in this routine; while after this PR the execution just returns back from resume() when node is stopped. This seems to cause no issue: resume() should be called after pause(), while stop() is chaind with start(). So, behavior should be even more correct causing no hangs at this case.
Uncommented the "Map update loop missed its desired rate" warning

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

Fixed formatting. Bring back update and publish of the costmap layers, making sure the cost map is reinitialized by implicit calling update function. Calling proper function. Canceling and reseting of the timer instead of recreation. After second round of review. Check against nullptr_t Typo Nullptr Linter

AlexeyMerzlyakov · 2023-02-10T13:27:45Z

kpawelczyk and others added 2 commits

I've cherry-picked original commit from kpawelczyk's branch to mine. @SteveMacenski, will this PR preserve original kpawelczyk authorship after merge, or am I done something wrong?

SteveMacenski · 2023-02-10T19:30:07Z

nav2_costmap_2d/src/costmap_2d_ros.cpp

+        map_update_frequency_);
+    }
+  } else {
+    map_update_timer_->reset();


if map_update_timer_ its a nullptr, why reset it?

I think that may actually be dereferencing a nullptr

If map_update_timer_ is a nullptr, we are trying to create a new timer, else - resetting existing timer:

if (!map_update_timer_) { // map_update_timer_ == nullptr try to create a new map_update_timer_ object } else { // map_update_timer_ != nullptr // which means that timer is already created. We just need to reset it: map_update_timer_->reset(); }

I looks like there are no cases where we could dereference a null pointer.

SteveMacenski · 2023-02-10T19:32:29Z

I thought we discussed that having the timer had other issues and that we were just going to update the rates away from wall to use simulation time? A rclcpp::Rate object instead of rclcpp::WallRate would do the job. Unless you figured out the Timer / callback starvation issue?

SteveMacenski · 2023-02-10T19:33:11Z

I've cherry-picked original commit from kpawelczyk's branch to mine. @SteveMacenski, will this PR preserve original kpawelczyk authorship after merge, or am I done something wrong?

Yes, the git logs look clean. They store that he's the author but you committed it

AlexeyMerzlyakov · 2023-02-14T14:51:56Z

A rclcpp::Rate object instead of rclcpp::WallRate would do the job. Unless you figured out the Timer / callback starvation issue?

It seems like rclcpp::Rate is another wrapper for GenericRate clock, but instead of WallRate, it uses steady clock:
https://github.com/ros2/rclcpp/blob/rolling/rclcpp/include/rclcpp/rate.hpp#L115
So, this won't be a solution in this case, since we need to respect simulation ROS clock.

However, this idea made me a think about the accuracy of solution I've presented before. After close double-check, I've discovered that this solution was using periodic granular std::this_thread::sleep_for(10ms); to wait, which gave us inaccuracies reported here. Instead of this, it is better to copy the RCLCPP's solution, but instead of using rclcpp::sleep_for(chrono time_to_sleep) it should use rclcpp::Clock->sleep_for(time_to_sleep); ROS-time respective API. This is an implementation of our own nav2_utils::ROSTimer, as latest experiments shown, has approximately the same time accuracy as proposed in this ticket rclcpp::timer approach.

I thought we discussed that having the timer had other issues and that we were just going to update the rates away from wall to use simulation time?

As of experiments, it seems that accuracy of this approach somewhat similar to having our own accurate nav2_utils::ROSTimer. However, if we will load the main thread with heavy PCL-calculation algorithms, your words might be confirmed, I've just never checked it.

From the said above, it looks like the solution to have a correct copy of a rclcpp::WallRate implementation in a nav2_utils::ROSTime still have to take place. @SteveMacenski, if you will give a green light, I'll change this PR to nav2_utils::ROSTime implementation.

SteveMacenski · 2023-02-14T18:27:34Z

Interesting. rclcpp::Timer uses the clock though https://github.com/ros2/rclcpp/blob/rolling/rclcpp/include/rclcpp/timer.hpp which can be set to ROS time. I'm surprised that Rate doesn't have that option. It might be worth filing a ticket with rclcpp to have the Rate be able to respect ROS time, which I looked into and I think could be possible if we made the clock a parameter option for construction and use. My guess is it wasn't done for two reasons: (1) this was an early feature of rclcpp and ROS time may not have been fully baked yet and (2) ROS times can stop or reverse so it complicates things if you're trying to retain a regularized rate of execution. Rate will block a thread versus the Timer which adds things to the callback queue.

Instead of this, it is better to copy the RCLCPP's solution, but instead of using rclcpp::sleep_for(chrono time_to_sleep) it should use rclcpp::Clock->sleep_for(time_to_sleep);

Maybe its worth actually just changing the Rate in rclcpp to support what we need instead? I have a call with the ROS Maintainers later today, I'll inquire about the interest of this.

nav2_utils::ROSTimer

Can you show me this "nav2_utils::ROSTimer"? I don't see it in the stack.

AlexeyMerzlyakov · 2023-02-15T09:10:42Z

Can you show me this "nav2_utils::ROSTimer"? I don't see it in the stack.

It is an implementation of rclcpp::GenericRate moved on ROS-time rails. This is a prototype not published yet, but here we are:

class ROSRate
{
public:
  ROSRate(double rate, rclcpp::Clock::SharedPtr clock)
  : ROSRate(rclcpp::Duration::from_seconds(1.0 / rate), clock)
  {}
  ROSRate(rclcpp::Duration period, rclcpp::Clock::SharedPtr clock)
  : period_(period), last_interval_(clock->now()), clock_(clock)
  {}

  bool sleep()
  {
    // Time coming into sleep
    rclcpp::Time now = clock_->now();
    // Time of next interval
    rclcpp::Time next_interval = last_interval_ + period_;
    // Detect backwards time flow
    if (now < last_interval_) {
      next_interval = now + period_;
    }

    // Update the interval
    last_interval_ += period_;

    // If time of next interval is earlier than now, don't sleep
    if (next_interval <= now) {
      // If an entire cycle was missed then reset next interval.
      // This might happen if the loop took more than a cycle.
      // Or if time jumps forward.
      if (now > next_interval + period_) {
        last_interval_ = now + period_;
      }
      // Either way do not sleep and return false
      return false;
    }

    // Calculate the time to sleep
    const rclcpp::Duration time_to_sleep = next_interval - now;
    // Sleep
    clock_->sleep_for(time_to_sleep);

    return true;
  }

  void reset()
  {
    last_interval_ = clock_->now();
  }

  rclcpp::Duration period() const
  {
    return period_;
  }

protected:
  rclcpp::Duration period_;
  rclcpp::Time last_interval_;
  rclcpp::Clock::SharedPtr clock_;
};

I have a call with the ROS Maintainers later today, I'll inquire about the interest of this.

Any news from RCLCPP team? If so, I am ready to submit a new feature request there.

SteveMacenski · 2023-02-15T20:26:51Z

See Slack - I got some feedback and resources from Tully and Chris about how to contribute a ROS Time version of Rate into rclcpp. There's interest in it so we don't have to maintain it here

tonynajjar · 2023-03-27T14:50:03Z

Could you please link to the continuation of this? Is there a PR for rclcpp?

AlexeyMerzlyakov · 2023-03-28T12:16:55Z

Yes, the proper way - is to add ROS-time respective rate to RCLCPP API. The according PR is: ros2/rclcpp#2123

SteveMacenski · 2023-06-05T19:44:15Z

What's the story here? Is the change being looked at for Rolling now?

AlexeyMerzlyakov · 2023-06-06T08:20:19Z

I've just created a new PR ros2/system_tests#516 in system_tests, fixing the test regressions noted in logs from CI verificaiton that was failed in ros2/rclcpp#2123. After PR516 will be merged, I expect no more test regressions in PR2123 appeared with new rclcpp::Rate API.

SteveMacenski · 2023-06-06T16:29:17Z

OK! Let me know if I can help

SteveMacenski · 2023-08-31T19:56:10Z

Yay! rclcpp is merged!

SteveMacenski · 2023-12-04T22:52:28Z

@AlexeyMerzlyakov from your analysis in #3325 (comment) with the higher variation of executions, is this ready to go (e.g. not suffering from this problem)?

I'm looking to address the timing issues myself this week, it looks like you covered this one but can you verify this doesn't suffer from that problem you previously mentioned and that this is good to go? Or, does it make sense to instead just change https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d_ros.cpp#L496 WallRate to the new Rate with clock API

AlexeyMerzlyakov · 2023-12-06T12:33:20Z

This PR is used as an alternative for updated rclcpp::Rate API, since costmaps are using rclcpp::WallRate, as you've mentioned. So, rclcpp::WallRate r(frequency); could be replaced to rclcpp::Rate r(this->get_clock(), frequency); and this PR is not needed.

Regarding the #3325 (comment) time variations - this was related to inaccurate waiting grain approach in proposed by #3325 (comment) solution

std::this_thread::sleep_for(10ms);  // Sleep grain is 10ms

which was finally refused. So this should not be an our case for now (although it still deserves performance re-measurement of new rclcpp::Rate vs rclcpp::create_timer() timing accuracy comparison).

SteveMacenski · 2023-12-06T19:23:25Z

Ok got it! Closing in favor of the new API then, thanks for the clarification and your help on this issue!

kpawelczyk and others added 2 commits February 6, 2023 11:05

Meet review items

927c480

SteveMacenski mentioned this pull request Feb 10, 2023

Map update in accordance to ROS time. #3327

Closed

7 tasks

SteveMacenski reviewed Feb 10, 2023

View reviewed changes

This was referenced Dec 4, 2023

Mixing Wall time with simulation time in Costmap2DROS::mapUpdateLoop #3325

Closed

Timed Behaviors do not adhere to Sim Time #3303

Closed

SteveMacenski closed this Dec 6, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Costmap update in accordance to ROS time #3404

Costmap update in accordance to ROS time #3404

AlexeyMerzlyakov commented Feb 10, 2023 •

edited

Loading

AlexeyMerzlyakov commented Feb 10, 2023 •

edited

Loading

SteveMacenski Feb 10, 2023

SteveMacenski Feb 10, 2023

AlexeyMerzlyakov Feb 15, 2023

SteveMacenski commented Feb 10, 2023

SteveMacenski commented Feb 10, 2023

AlexeyMerzlyakov commented Feb 14, 2023 •

edited

Loading

SteveMacenski commented Feb 14, 2023

AlexeyMerzlyakov commented Feb 15, 2023 •

edited

Loading

SteveMacenski commented Feb 15, 2023

tonynajjar commented Mar 27, 2023

AlexeyMerzlyakov commented Mar 28, 2023 •

edited

Loading

SteveMacenski commented Jun 5, 2023

AlexeyMerzlyakov commented Jun 6, 2023 •

edited

Loading

SteveMacenski commented Jun 6, 2023

SteveMacenski commented Aug 31, 2023

SteveMacenski commented Dec 4, 2023 •

edited

Loading

AlexeyMerzlyakov commented Dec 6, 2023 •

edited

Loading

SteveMacenski commented Dec 6, 2023

Costmap update in accordance to ROS time #3404

Costmap update in accordance to ROS time #3404

Conversation

AlexeyMerzlyakov commented Feb 10, 2023 • edited Loading

Basic Info

Description of contribution in a few bullet points

For Maintainers:

AlexeyMerzlyakov commented Feb 10, 2023 • edited Loading

SteveMacenski Feb 10, 2023

Choose a reason for hiding this comment

SteveMacenski Feb 10, 2023

Choose a reason for hiding this comment

AlexeyMerzlyakov Feb 15, 2023

Choose a reason for hiding this comment

SteveMacenski commented Feb 10, 2023

SteveMacenski commented Feb 10, 2023

AlexeyMerzlyakov commented Feb 14, 2023 • edited Loading

SteveMacenski commented Feb 14, 2023

AlexeyMerzlyakov commented Feb 15, 2023 • edited Loading

SteveMacenski commented Feb 15, 2023

tonynajjar commented Mar 27, 2023

AlexeyMerzlyakov commented Mar 28, 2023 • edited Loading

SteveMacenski commented Jun 5, 2023

AlexeyMerzlyakov commented Jun 6, 2023 • edited Loading

SteveMacenski commented Jun 6, 2023

SteveMacenski commented Aug 31, 2023

SteveMacenski commented Dec 4, 2023 • edited Loading

AlexeyMerzlyakov commented Dec 6, 2023 • edited Loading

SteveMacenski commented Dec 6, 2023

AlexeyMerzlyakov commented Feb 10, 2023 •

edited

Loading

AlexeyMerzlyakov commented Feb 10, 2023 •

edited

Loading

AlexeyMerzlyakov commented Feb 14, 2023 •

edited

Loading

AlexeyMerzlyakov commented Feb 15, 2023 •

edited

Loading

AlexeyMerzlyakov commented Mar 28, 2023 •

edited

Loading

AlexeyMerzlyakov commented Jun 6, 2023 •

edited

Loading

SteveMacenski commented Dec 4, 2023 •

edited

Loading

AlexeyMerzlyakov commented Dec 6, 2023 •

edited

Loading