Migrate Nav2 timers & rates to use steady clocks and be sim-time-aware

nav2_amcl/src/amcl_node.cpp

@@ -23,9 +23,9 @@
 #include "nav2_amcl/amcl_node.hpp"
 
 #include <algorithm>
+#include <cstdint>
 #include <cstdio>
 #include <ctime>
-#include <cstdint>
 #include <iomanip>
 #include <memory>
 #include <string>
@@ -148,7 +148,7 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   // Create pose save timer if save_pose_rate > 0
   if (save_pose_rate_ > 0.0) {
-    save_pose_timer_ = create_wall_timer(
+    save_pose_timer_ = this->create_timer(
       std::chrono::duration<double>(1.0 / save_pose_rate_),
       std::bind(&AmclNode::savePoseTimerCallback, this));
   }

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -141,8 +141,8 @@ class BehaviorTreeEngine
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
 
-  // Clock
-  rclcpp::Clock::SharedPtr clock_;
+  // Node handle used to obtain clocks at run time
+  nav2::LifecycleNode::WeakPtr node_;
 
   // Groot2 monitor
   std::unique_ptr<BT::Groot2Publisher> groot_monitor_;

nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp

@@ -16,6 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__UTILS__LOOP_RATE_HPP_
 
 #include <memory>
+#include <utility>
 
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/bt_factory.h"
@@ -27,8 +28,10 @@ namespace nav2_behavior_tree
 class LoopRate
 {
 public:
-  LoopRate(const rclcpp::Duration & period, BT::Tree * tree)
-  : clock_(std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME)), period_(period),
+  LoopRate(
+    const rclcpp::Duration & period, BT::Tree * tree,
+    rclcpp::Clock::SharedPtr clock)
+  : clock_(std::move(clock)), period_(period),
     last_interval_(clock_->now()), tree_(tree)
   {}
 
@@ -57,11 +60,16 @@ class LoopRate
       // Either way do not sleep and return false
       return false;
     }
-    // Calculate the time to sleep
-    auto time_to_sleep = next_interval - now;
-    std::chrono::nanoseconds time_to_sleep_ns(time_to_sleep.nanoseconds());
-    // Sleep (can get interrupted by emitWakeUpSignal())
-    tree_->sleep(std::chrono::duration_cast<std::chrono::microseconds>(time_to_sleep_ns));
+    auto wake_up = tree_->wakeUpSignal();
+    static constexpr auto poll_interval = std::chrono::milliseconds(10);
+    while (clock_->now() < next_interval) {
+      // Sleep using the wake-up signal directly so we can poll the target clock.
+      // tree_->sleep() always waits in wall-clock time, which diverges from sim
+      // time.  Instead we do short wall-clock waits and re-check our clock.
+      if (wake_up->waitFor(poll_interval)) {  // Preempted by emitWakeUpSignal()
+        return true;
+      }
+    }
     return true;
   }
 

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -26,6 +26,7 @@
 #include "behaviortree_cpp/utils/shared_library.h"
 #include "nav2_behavior_tree/json_utils.hpp"
 #include "nav2_behavior_tree/utils/loop_rate.hpp"
+#include "nav2_ros_common/rate.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -39,8 +40,7 @@ BehaviorTreeEngine::BehaviorTreeEngine(
     factory_.registerFromPlugin(loader.getOSName(p));
   }
 
-  // clock for throttled debug log
-  clock_ = node->get_clock();
+  node_ = node;
 }
 
 BtStatus
@@ -50,7 +50,16 @@ BehaviorTreeEngine::run(
   std::function<bool()> cancelRequested,
   std::chrono::milliseconds loopTimeout)
 {
-  nav2_behavior_tree::LoopRate loopRate(loopTimeout, tree);
+  auto node = node_.lock();
+  if (!node) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("BehaviorTreeEngine"),
+      "BehaviorTreeEngine node expired. Exiting with failure.");
+    return BtStatus::FAILED;
+  }
+
+  auto rate_clock = nav2::selectClock(node);
+  nav2_behavior_tree::LoopRate loopRate(loopTimeout, tree, rate_clock);
   BT::NodeStatus result = BT::NodeStatus::RUNNING;
 
   // Loop until something happens with ROS or the node completes
@@ -70,7 +79,7 @@ BehaviorTreeEngine::run(
       if (!loopRate.sleep()) {
         RCLCPP_DEBUG_THROTTLE(
           rclcpp::get_logger("BehaviorTreeEngine"),
-          *clock_, 1000,
+          *rate_clock, 1000,
           "Behavior Tree tick rate %0.2f was exceeded!",
           1.0 / (loopRate.period().count() * 1.0e-9));
       }

nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp

@@ -267,7 +267,8 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success)
   BT::NodeStatus result = BT::NodeStatus::RUNNING;
 
   // BT loop execution rate
-  nav2_behavior_tree::LoopRate loopRate(10ms, tree_.get());
+  nav2_behavior_tree::LoopRate loopRate(
+    10ms, tree_.get(), std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME));
 
   // main BT execution loop
   while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {

nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -32,6 +32,7 @@
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/twist_publisher.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
+#include "nav2_ros_common/rate.hpp"
 #include "nav2_core/behavior.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -226,7 +227,7 @@ class TimedBehavior : public nav2_core::Behavior
     }
 
     auto start_time = clock_->now();
-    rclcpp::WallRate loop_rate(cycle_frequency_);
+    nav2::Rate loop_rate(node_.lock(), cycle_frequency_);
 
     while (rclcpp::ok()) {
       elapsed_time_ = clock_->now() - start_time;

nav2_controller/src/controller_server.cpp

@@ -22,6 +22,7 @@
 #include "lifecycle_msgs/msg/state.hpp"
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
+#include "nav2_ros_common/rate.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/path_utils.hpp"
 #include "nav2_controller/controller_server.hpp"
@@ -494,7 +495,7 @@ void ControllerServer::computeControl()
     }
 
     last_valid_cmd_time_ = now();
-    rclcpp::WallRate loop_rate(params_->controller_frequency);
+    nav2::Rate loop_rate(this, params_->controller_frequency);
     while (rclcpp::ok()) {
       auto start_time = this->now();
 

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -47,6 +47,7 @@
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_util/execution_timer.hpp"
 #include "nav2_ros_common/node_utils.hpp"
+#include "nav2_ros_common/rate.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2_ros/create_timer_ros.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -532,7 +533,7 @@ Costmap2DROS::mapUpdateLoop(double frequency)
 
   RCLCPP_DEBUG(get_logger(), "Entering loop");
 
-  rclcpp::WallRate r(frequency);    // 200ms by default
+  nav2::Rate r(this, frequency);    // 200ms by default
 
   while (rclcpp::ok() && !map_update_thread_shutdown_) {
     nav2_util::ExecutionTimer timer;

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "angles/angles.h"
+#include "nav2_ros_common/rate.hpp"
 #include "opennav_docking/docking_server.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/utils.hpp"
@@ -175,7 +176,7 @@ void DockingServer::dockRobot()
 {
   std::lock_guard<std::mutex> lock_reinit(param_handler_->getMutex());
   action_start_time_ = this->now();
-  rclcpp::Rate loop_rate(params_->controller_frequency);
+  nav2::Rate loop_rate(this, params_->controller_frequency);
 
   auto goal = docking_action_server_->get_current_goal();
   auto result = std::make_shared<DockRobot::Result>();
@@ -393,7 +394,7 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta
     throw opennav_docking_core::FailedToDetectDock("Failed to start the detection process.");
   }
 
-  rclcpp::Rate loop_rate(params_->controller_frequency);
+  nav2::Rate loop_rate(this, params_->controller_frequency);
   auto start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(params_->initial_perception_timeout);
   while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) {
@@ -419,7 +420,7 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po
   target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
     tf2::getYaw(target_pose.pose.orientation) + M_PI);
 
-  rclcpp::Rate loop_rate(params_->controller_frequency);
+  nav2::Rate loop_rate(this, params_->controller_frequency);
   auto start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(params_->rotate_to_dock_timeout);
 
@@ -452,7 +453,7 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po
 bool DockingServer::approachDock(
   Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward)
 {
-  rclcpp::Rate loop_rate(params_->controller_frequency);
+  nav2::Rate loop_rate(this, params_->controller_frequency);
   auto start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(params_->dock_approach_timeout);
 
@@ -522,7 +523,7 @@ bool DockingServer::waitForCharge(Dock * dock)
     return true;
   }
 
-  rclcpp::Rate loop_rate(params_->controller_frequency);
+  nav2::Rate loop_rate(this, params_->controller_frequency);
   auto start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(params_->wait_charge_timeout);
   while (rclcpp::ok()) {
@@ -550,7 +551,7 @@ bool DockingServer::waitForCharge(Dock * dock)
 bool DockingServer::resetApproach(
   const geometry_msgs::msg::PoseStamped & staging_pose, bool backward)
 {
-  rclcpp::Rate loop_rate(params_->controller_frequency);
+  nav2::Rate loop_rate(this, params_->controller_frequency);
   auto start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(params_->dock_approach_timeout);
   while (rclcpp::ok()) {
@@ -621,7 +622,7 @@ void DockingServer::undockRobot()
 {
   std::lock_guard<std::mutex> lock_reinit(param_handler_->getMutex());
   action_start_time_ = this->now();
-  rclcpp::Rate loop_rate(params_->controller_frequency);
+  nav2::Rate loop_rate(this, params_->controller_frequency);
 
   auto goal = undocking_action_server_->get_current_goal();
   auto result = std::make_shared<UndockRobot::Result>();

nav2_following/opennav_following/src/following_server.cpp

@@ -14,6 +14,7 @@
 // limitations under the License.
 
 #include "angles/angles.h"
+#include "nav2_ros_common/rate.hpp"
 #include "opennav_docking_core/docking_exceptions.hpp"
 #include "opennav_following/following_server.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -180,7 +181,7 @@ void FollowingServer::followObject()
 {
   std::lock_guard<std::mutex> lock_reinit(param_handler_->getMutex());
   action_start_time_ = this->now();
-  rclcpp::Rate loop_rate(params_->controller_frequency);
+  nav2::Rate loop_rate(this, params_->controller_frequency);
 
   auto goal = following_action_server_->get_current_goal();
   auto result = std::make_shared<FollowObject::Result>();
@@ -525,7 +526,7 @@ bool FollowingServer::getRefinedPose(geometry_msgs::msg::PoseStamped & pose)
   if (detected.header.stamp == rclcpp::Time(0)) {
     auto start = this->now();
     auto timeout = rclcpp::Duration::from_seconds(params_->detection_timeout);
-    rclcpp::Rate wait_rate(params_->controller_frequency);
+    nav2::Rate wait_rate(this, params_->controller_frequency);
     while (this->now() - start < timeout) {
       // Check if a new detection arrived
       if (detected_dynamic_pose_.header.stamp != rclcpp::Time(0)) {

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -21,6 +21,7 @@
 #include <vector>
 
 #include "rclcpp/rclcpp.hpp"
+#include "nav2_ros_common/interface_factories.hpp"
 
 using namespace std::chrono_literals;
 using namespace std::placeholders;
@@ -73,15 +74,17 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
     std::string("Shutting down ");
 
-  init_timer_ = this->create_wall_timer(
+  init_timer_ = nav2::create_timer(
+    this,
     0s,
     [this]() -> void {
       init_timer_->cancel();
       createLifecyclePublishers();
       createLifecycleServiceClients();
       createLifecycleServiceServers();
       if (autostart_) {
-        init_timer_ = this->create_wall_timer(
+        init_timer_ = nav2::create_timer(
+          this,
           0s,
           [this]() -> void {
             init_timer_->cancel();
@@ -481,7 +484,8 @@ LifecycleManager::createBondTimer()
   }
 
   message("Creating bond timer...");
-  bond_timer_ = this->create_wall_timer(
+  bond_timer_ = nav2::create_timer(
+    this,
     200ms,
     std::bind(&LifecycleManager::checkBondConnections, this),
     callback_group_);
@@ -556,7 +560,8 @@ LifecycleManager::checkBondConnections()
       // Initialize the bond respawn timer to check if server comes back online
       // after a failure, within a maximum timeout period.
       if (attempt_respawn_reconnection_) {
-        bond_respawn_timer_ = this->create_wall_timer(
+        bond_respawn_timer_ = nav2::create_timer(
+          this,
           1s,
           std::bind(&LifecycleManager::checkBondRespawnConnection, this),
           callback_group_);

nav2_lifecycle_manager/test/test_bond.cpp

@@ -19,6 +19,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/node_thread.hpp"
+#include "nav2_ros_common/rate.hpp"
 #include "nav2_lifecycle_manager/lifecycle_manager.hpp"
 #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
 
@@ -128,7 +129,7 @@ class TestFixture
 TEST(LifecycleBondTest, POSITIVE)
 {
   // let the lifecycle server come up
-  rclcpp::Rate(1).sleep();
+  nav2::WallRate(1).sleep();
 
   auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
   nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
@@ -140,14 +141,14 @@ TEST(LifecycleBondTest, POSITIVE)
   EXPECT_TRUE(client.startup());
 
   // check if bond is connected after being activated
-  rclcpp::Rate(5).sleep();
+  nav2::WallRate(5).sleep();
   EXPECT_TRUE(bond_tester->isBondConnected());
   EXPECT_EQ(bond_tester->getState(), "activated");
 
   bond_tester->breakBond();
 
   // bond should be disconnected now and lifecycle manager should know and react to reset
-  rclcpp::Rate(5).sleep();
+  nav2::WallRate(5).sleep();
   EXPECT_EQ(
     nav2_lifecycle_manager::SystemStatus::INACTIVE,
     client.is_active(std::chrono::nanoseconds(1000000000)));