Change clocks to use sim-time aware ROS Time

[tonynajjar]

Basic Info

Ticket(s) this addresses	#5917
Primary OS tested on	Ubuntu 24.04
Robotic platform tested on	Gazebo & loopback simulation
Does this PR contain AI generated software?	Yes
Was this PR description generated by AI software?	Yes but revised

Description of contribution in a few bullet points

• Rate-limited control loops now use ROS time in simulation, steady time on real hardware. The ControllerServer, VelocitySmoother and Costmap2DROS update loops select their clock based on use_sim_time: the node's ROS clock (follows /clock) when true, or a monotonic RCL_STEADY_TIME clock when false. This makes the loops sim-time-aware while remaining immune to NTP clock jumps on real hardware.

• All other production rclcpp::Rate and rclcpp::WallRate usages in continuous control/update loops migrated to use the node clock (this->get_clock()), making them sim-time-aware:

  • TimedBehavior (behaviors)
  • DockingServer (7 loops)
  • FollowingServer (4 loops)
  • RouteTracker
  • WaypointFollower
  • InputAtWaypoint
    When not using sim-time, this will fallback to system time but we accept this as these components depend less crucially on monotonic precise timing than the components mentioned above

• create_wall_timer → create_timer in AmclNode (save pose timer) and VelocitySmoother (smoother timer), so these timers respect sim time.

• BT LoopRate updated to accept an injected clock and use a polling loop via tree->wakeUpSignal() instead of tree->sleep(), which was hardwired to wall-clock time. When running with sim time, the loop polls in short wall-clock intervals and re-checks the target clock; for steady/system clocks it waits for the exact remaining duration.

• No new abstractions introduced. The clock selection is done inline at the two call sites that need it (controller server and costmap), using a straightforward pattern:

  auto loop_clock = get_parameter("use_sim_time").as_bool() ?
    this->get_clock() :
    std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME);

• Polling/waiting loops and wall-time-bound operations left on default clock. rclcpp::Rate usages that poll for readiness (TF availability, costmap initialization, DDS discovery, bond heartbeats) or wait for CPU/network-bound operations don't need sim-time awareness and are left unchanged. Only continuous control and update loops are migrated.

• Test files left unchanged except for system tests that run with Gazebo (assisted_teleop_behavior_tester, wait_behavior_tester) where rclcpp::Rate is used in a control-like loop with use_sim_time=true.

Dependencies

Requires BehaviorTree.CPP#1127 (Tree::wakeUpSignal() getter) — already merged.

Description of documentation updates required from your changes

None. No new parameters, plugins, or public API changes.

Description of how this change was tested

TODO

Future work that may be required in bullet points

[tonynajjar]

To be tested! I opened the PR to see if I find obvious issues in CI first

[codecov]

Codecov Report

❌ Patch coverage is 82.60870% with 12 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...ehaviors/include/nav2_behaviors/timed_behavior.hpp	25.00%	6 Missing ⚠️
nav2_behavior_tree/src/behavior_tree_engine.cpp	60.00%	2 Missing ⚠️
...v2_waypoint_follower/plugins/input_at_waypoint.cpp	50.00%	2 Missing ⚠️
nav2_loopback_sim/src/loopback_simulator.cpp	75.00%	1 Missing ⚠️
nav2_route/src/route_tracker.cpp	66.66%	1 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_amcl/src/amcl_node.cpp	89.56% <100.00%> (ø)
...nclude/nav2_behavior_tree/behavior_tree_engine.hpp	100.00% <ø> (ø)
...ree/include/nav2_behavior_tree/utils/loop_rate.hpp	95.65% <100.00%> (-4.35%)	⬇️
nav2_controller/src/controller_server.cpp	80.39% <100.00%> (ø)
nav2_costmap_2d/src/costmap_2d_ros.cpp	90.22% <100.00%> (-0.26%)	⬇️
...av2_docking/opennav_docking/src/docking_server.cpp	89.48% <100.00%> (ø)
...llowing/opennav_following/src/following_server.cpp	90.30% <100.00%> (ø)
nav2_lifecycle_manager/src/lifecycle_manager.cpp	90.35% <100.00%> (ø)
...on/include/nav2_ros_common/interface_factories.hpp	93.33% <100.00%> (+0.22%)	⬆️
..._common/include/nav2_ros_common/lifecycle_node.hpp	92.39% <100.00%> (+0.16%)	⬆️
... and 9 more

... and 9 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[tonynajjar]

One risk I see with this PR:

The default clock type of rclcpp::Rate is RCL_SYSTEM_TIME and of rclcpp::WallRate is RCL_STEADY_TIME

By replacing rclcpp::WallRate and rclcpp::Rate with rclcpp::Rate(freq, clock), we're changing the default clock type to be that of the node's clock which is RCL_ROS_TIME. Now this is not a problem for rclcpp::Rate because when use_sim_time=false, RCL_ROS_TIME behaves like RCL_SYSTEM_TIME.

However for rclcpp::WallRate we're changing from RCL_STEADY_TIME to RCL_SYSTEM_TIME. The difference between these clock types can be found here

I do think that e.g. controller loop should be using RCL_STEADY_TIME i.e. it shouldn't be affected by e.g. NTP changes. What do you think?

[SteveMacenski]

Especially for the controller server, costmap rates, can you comment on the previous discussions on this subject why we left them the way that they are?

I do think that e.g. controller loop should be using RCL_STEADY_TIME i.e. it shouldn't be affected by e.g. NTP changes. What do you think?

This is a key part of that, I think. As well as for many algorithms that cannot/should not be run at multiples of rate in simulation due to inability to realistically ever keep up - causing incorrect behaviors when running at > 1x speed which are not deterministic at 1x speed.

I mentioned in the ticket that #3325 (comment) is a good summary of the current state

[tonynajjar]

Especially for the controller server, costmap rates, can you comment on the previous discussions on this subject why we left them the way that they are?

Regarding your previous conclusion here:

This is because systems like the costmap and controller server are not systems that can be simulated at 2x-3x-10x speeds due to their computational complexity.

I mean it highly depends on the computational complexity of the plugins and the machine's specs no? Without hard numbers yet, but it seems like I can run MPPI on 10 Hz at 5x speed on my Intel(R) Core(TM) Ultra 9 285H.

It also created a number of serious issues breaking testing behavior which given the above statements (which almost certainly impacts real simulation use-cases as well), after about half a day of debugging I ultimately decided wasn't worth continuing to address.

In case you remember, would be helpful to share more details so I can try to reproduce. The setup I tested so far is quite a simple one with loopback simulation

I do think that e.g. controller loop should be using RCL_STEADY_TIME i.e. it shouldn't be affected by e.g. NTP changes. What do you think?

For this, it's not a blocker, we should just find out a way to switch between "/clock time" and RCL_STEADY_TIME instead of RCL_SYSTEM_TIME (As mentioned, a RCL_ROS_TIME clock will currently choose between "/clock time" and RCL_SYSTEM_TIME according to use_sim_time). I wonder if that should be a feature upstream eventually but on the short term, it's as simple as:

clock = use_sim_time ? node->get_clock() : std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME)   
rclcpp::Rate loop_rate(frequency, clock)

[SteveMacenski]

I don't recall the specific issues I ran into from that long ago unfortunately. I do remember frustrating number of system tests became flaky though. I'll rerun CI a few times and see if that's still the case here.

I do think that e.g. controller loop should be using RCL_STEADY_TIME i.e. it shouldn't be affected by e.g. NTP changes.

On this point for TimedBehavior, ControllerServer, Costmap2DROS, VelocitySmoother, and WaypointFollower: I generally agree. However, I think that the following is too brute force in the application code

clock = use_sim_time ? node->get_clock() : std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME)   

I don't think we can set the clock type when not simulation time in the NodeOptions unfortunately. Maybe though something to consider adding?

The next best thing would be to create a nav2_ros_common::Rate which is used instead globally. This would use simulation time when necessary or the RCL_STEADY_TIME otherwise. Removes the boilerplate from in-line in the code + continues to pattern of abstracting ROS 2 APIs into that package so we can make Nav2-specific customizations like this now and into the future. Just add a comment to mention this NTP bit in the doxygen and we're golden.

Are there any cases for Rate we want to use NTP corrections? I kind of doubt it.

---

Maybe missed a few?

• navigation2/nav2_amcl/src/amcl_node.cpp

  Line 151 in 0c62720

  save_pose_timer_ = create_wall_timer(

• navigation2/nav2_lifecycle_manager/src/lifecycle_manager.cpp

  Line 76 in 0c62720

  init_timer_ = this->create_wall_timer(

• navigation2/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp

  Line 306 in 0c62720

  autostart_timer_ = this->create_wall_timer(

• navigation2/nav2_core/include/nav2_core/behavior_tree_navigator.hpp

  Line 336 in d85340a

  rclcpp::Rate r(100);

• navigation2/nav2_util/include/nav2_util/lifecycle_service_client.hpp

  Line 48 in d85340a

  rclcpp::Rate r(20);

---

Given the BT.CPP changes required, can we change this to a draft until that is merged by Davide?

[tonynajjar]

Given the BT.CPP changes required, can we change this to a draft until that is merged by Davide?

it's merged already :D We still need to keep BT.CPP in the vendor.repos until next bloom release I presume

[tonynajjar]

btw looks like the Jazzy and Kilted workflows don't build the underlay from vendor.repos? Does that mean that if this get merged the workflows will remain broken? Is that new to you?

[SteveMacenski]

Yes, and that is technically expected. I'm on the fence about it, but tentatively going to update the job to use the repos file for now.

The main issue is that what happens if changes are made to a key dependency like BT.CPP which are not backward compatible and cannot/will not be updated in Humble or Jazzy. Our CI works for those distributions, but users themselves cannot do it without also updating something like BT.CPP which from v3 to v4 may fundamentally not work with their existing code.

That's not an issue today since Davide appears to release v4 up to date in every ROS distribution, so I'll kick this can down the road for another time if this becomes an issue. Here's a PR to resolve: #6069

[tonynajjar]

ros2/rclcpp#3122 made an issue for rclcpp but will will need to find a local solution in the meantime - I'm working on that

[tonynajjar]

Don't review yet

[tonynajjar]

Actually in terms of changes that's pretty much it so you can take a look already. Just didn't have the time to make a nice description but basically, with the help of some custom Nav2 wrappers of WallRate and create_wall_timer, I made sure that we use RCL_STEADY_CLOCK when use_sim_time is false and /clock when use_sim_time is true. I did that in the node - I don't think the tests need to change but to be seen.

[SteveMacenski]

Otherwise the code that's changed LGTM.

I will say though that I need to look at this again with fresh eyes. Does this imply that we should actually update all Rate's or Timers to use this as well? Is there a downside to that in any case? I do like the idea of abstracting even more rclcpp::'s into nav2::'s but only if that's really the right answer.

[Copilot]

Pull request overview

Updates Nav2 timing primitives to be robust to system clock jumps and simulation-time execution by selecting an appropriate clock (ROS/sim clock when use_sim_time=true, otherwise steady/monotonic time) for rates and timers.

Changes:

• Added nav2::selectClock() and nav2::WallRate and migrated several rate-limited loops to use it.
• Added sim-time-aware create_wall_timer helpers (free function + nav2::LifecycleNode shadow) and migrated LifecycleManager timers.
• Updated BT execution loop (LoopRate + BehaviorTreeEngine) to use an injected clock and a wakeup-driven wait loop.

Reviewed changes

Copilot reviewed 14 out of 14 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
tools/underlay.repos	Pins BehaviorTree.CPP to a specific commit to pick up Tree::wakeUpSignal() support.
nav2_waypoint_follower/src/waypoint_follower.cpp	Migrates waypoint follower loop rate to nav2::WallRate.
nav2_ros_common/include/nav2_ros_common/wall_rate.hpp	Introduces nav2::selectClock() and nav2::WallRate for sim-time-aware rate limiting.
nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp	Shadows create_wall_timer() to use the selected clock for lifecycle nodes.
nav2_ros_common/include/nav2_ros_common/interface_factories.hpp	Adds nav2::create_wall_timer() helper using clock selection.
nav2_lifecycle_manager/src/lifecycle_manager.cpp	Migrates timers (init_timer_, bond timers) to nav2::create_wall_timer().
nav2_costmap_2d/src/costmap_2d_ros.cpp	Migrates costmap update loop to nav2::WallRate.
nav2_controller/src/controller_server.cpp	Migrates controller compute loop to nav2::WallRate.
nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp	Migrates behavior cycle loop to nav2::WallRate.
nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp	Updates test to new LoopRate constructor signature (explicit clock).
nav2_behavior_tree/src/behavior_tree_engine.cpp	Injects selected clock into BT loop rate handling.
nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp	Reworks sleeping to poll an injected clock using BT wakeup signal.
nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp	Adds rate_clock_ member to support sim/steady timing for BT loop.
nav2_amcl/src/amcl_node.cpp	Minor include cleanup and explicitly calls this->create_wall_timer().

[SteveMacenski]

Otherise just this topic:

Does this imply that we should actually update all Rate's or Timers to use this as well? Is there a downside to that in any case? I do like the idea of abstracting even more rclcpp::'s into nav2::'s but only if that's really the right answer.

[tonynajjar]

Does this imply that we should actually update all Rate's or Timers to use this as well? Is there a downside to that in any case?

I'm going to use the terminology in ros2/rclcpp#3122 (comment)

For rates and timers, I think always RCL_ROS_STEADY_TIME is wanted over RCL_ROS_TIME or RCL_SYSTEM_TIME but probably there are a few places, e.g. in tests or maybe rviz panels, where we need RCL_STEADY_TIME (not sim-time aware).

I would need to do a deep dive

[SteveMacenski]

I think as part of this, we should, since its an easy grep + replace if it turns out there's no real reason to use the other now.

[tonynajjar]

Ok I did another pass and found more rclcpp::Rate to convert to nav2::WallRate. Now the remaining rclcpp::Rate are all in tests and mostly use like this rclcpp::Rate(1).sleep(). We have 4 choices:

• Keep as-is rclcpp::Rate. Not consistent and theoretically wrong to use system time but unlikely to practically matter for tests
• Use rclcpp::WallRate i.e. RCL_STEADY_TIME
• Create a nav2::X that represents RCL_STEADY_TIME to be consistent in using nav2:: overrides
• Use nav2::WallRate i.e. RCL_ROS_STEADY_TIME which is consistent across the stack but is sim-time aware which is perhaps a weird choice because tests don't run with sim time. The biggest drawback of this one is that we need to pass a node, which might not always be available in tests.

I'm torn because they all have some drawbacks but on the other hand whatever we choose is a (hopefully) temporary option until ros2/rclcpp#3122 in implemented. Because of that, I tend towards option 1