Cherry-pick from 15c9be0 (ros-navigation#4317)

Convert all wall timers and wall rates to ROS clock respecting rates and timers (ros-navigation#4000)

* Convert all wall timers and wall rates to ROS clock respecting rates and timers

* linty mclint face

* WPF wait plugin respect time

* move duration metrics to use local clocks

* bumping version for cache to break it

* complete timing refactor

* remove old variable

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -25,6 +25,7 @@
 #include "behaviortree_cpp_v3/xml_parsing.h"
 #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
 
+#include "rclcpp/rclcpp.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -46,7 +47,8 @@ class BehaviorTreeEngine
    * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine
    * @param plugin_libraries vector of BT plugin library names to load
    */
-  explicit BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries);
+  explicit BehaviorTreeEngine(
+    const std::vector<std::string> & plugin_libraries);
   virtual ~BehaviorTreeEngine() {}
 
   /**

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -25,7 +25,8 @@
 namespace nav2_behavior_tree
 {
 
-BehaviorTreeEngine::BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries)
+BehaviorTreeEngine::BehaviorTreeEngine(
+  const std::vector<std::string> & plugin_libraries)
 {
   BT::SharedLibrary loader;
   for (const auto & p : plugin_libraries) {

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -74,7 +74,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     command_speed_ = command->speed;
     command_time_allowance_ = command->time_allowance;
 
-    end_time_ = this->steady_clock_.now() + command_time_allowance_;
+    end_time_ = this->clock_->now() + command_time_allowance_;
 
     if (!nav2_util::getCurrentPose(
         initial_pose_, *this->tf_, this->global_frame_, this->robot_base_frame_,
@@ -93,7 +93,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
    */
   ResultStatus onCycleUpdate()
   {
-    rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now();
+    rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
     if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
       this->stopRobot();
       RCLCPP_WARN(

nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -117,8 +117,8 @@ class TimedBehavior : public nav2_core::Behavior
   {
     node_ = parent;
     auto node = node_.lock();
-
     logger_ = node->get_logger();
+    clock_ = node->get_clock();
 
     RCLCPP_INFO(logger_, "Configuring %s", name.c_str());
 
@@ -184,7 +184,7 @@ class TimedBehavior : public nav2_core::Behavior
   rclcpp::Duration elasped_time_{0, 0};
 
   // Clock
-  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
+  rclcpp::Clock::SharedPtr clock_;
 
   // Logger
   rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")};
@@ -215,11 +215,12 @@ class TimedBehavior : public nav2_core::Behavior
       return;
     }
 
-    auto start_time = steady_clock_.now();
+    auto start_time = clock_->now();
+
     rclcpp::WallRate loop_rate(cycle_frequency_);
 
     while (rclcpp::ok()) {
-      elasped_time_ = steady_clock_.now() - start_time;
+      elasped_time_ = clock_->now() - start_time;
       if (action_server_->is_cancel_requested()) {
         RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str());
         stopRobot();
@@ -236,7 +237,7 @@ class TimedBehavior : public nav2_core::Behavior
           " however feature is currently not implemented. Aborting and stopping.",
           behavior_name_.c_str());
         stopRobot();
-        result->total_elapsed_time = steady_clock_.now() - start_time;
+        result->total_elapsed_time = clock_->now() - start_time;
         action_server_->terminate_current(result);
         onActionCompletion();
         return;
@@ -248,14 +249,14 @@ class TimedBehavior : public nav2_core::Behavior
           RCLCPP_INFO(
             logger_,
             "%s completed successfully", behavior_name_.c_str());
-          result->total_elapsed_time = steady_clock_.now() - start_time;
+          result->total_elapsed_time = clock_->now() - start_time;
           action_server_->succeeded_current(result);
           onActionCompletion();
           return;
 
         case Status::FAILED:
           RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str());
-          result->total_elapsed_time = steady_clock_.now() - start_time;
+          result->total_elapsed_time = clock_->now() - start_time;
           result->error_code = on_cycle_update_result.error_code;
           action_server_->terminate_current(result);
           onActionCompletion();

nav2_behaviors/plugins/assisted_teleop.cpp

@@ -67,7 +67,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAct
 {
   preempt_teleop_ = false;
   command_time_allowance_ = command->time_allowance;
-  end_time_ = steady_clock_.now() + command_time_allowance_;
+  end_time_ = this->clock_->now() + command_time_allowance_;
   return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionGoal::NONE};
 }
 
@@ -82,7 +82,7 @@ ResultStatus AssistedTeleop::onCycleUpdate()
   feedback_->current_teleop_duration = elasped_time_;
   action_server_->publish_feedback(feedback_);
 
-  rclcpp::Duration time_remaining = end_time_ - steady_clock_.now();
+  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
   if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
     stopRobot();
     RCLCPP_WARN_STREAM(
@@ -125,7 +125,7 @@ ResultStatus AssistedTeleop::onCycleUpdate()
       if (time == simulation_time_step_) {
         RCLCPP_DEBUG_STREAM_THROTTLE(
           logger_,
-          steady_clock_,
+          *clock_,
           1000,
           behavior_name_.c_str() << " collided on first time step, setting velocity to zero");
         scaled_twist.linear.x = 0.0f;
@@ -135,7 +135,7 @@ ResultStatus AssistedTeleop::onCycleUpdate()
       } else {
         RCLCPP_DEBUG_STREAM_THROTTLE(
           logger_,
-          steady_clock_,
+          *clock_,
           1000,
           behavior_name_.c_str() << " collision approaching in " << time << " seconds");
         double scale_factor = time / projection_time_;

nav2_behaviors/plugins/back_up.cpp

@@ -31,7 +31,7 @@ ResultStatus BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> comma
   command_speed_ = -std::fabs(command->speed);
   command_time_allowance_ = command->time_allowance;
 
-  end_time_ = steady_clock_.now() + command_time_allowance_;
+  end_time_ = this->clock_->now() + command_time_allowance_;
 
   if (!nav2_util::getCurrentPose(
       initial_pose_, *tf_, global_frame_, robot_base_frame_,

nav2_behaviors/plugins/spin.cpp

@@ -91,14 +91,14 @@ ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)
     cmd_yaw_);
 
   command_time_allowance_ = command->time_allowance;
-  end_time_ = steady_clock_.now() + command_time_allowance_;
+  end_time_ = this->clock_->now() + command_time_allowance_;
 
   return ResultStatus{Status::SUCCEEDED, SpinActionGoal::NONE};
 }
 
 ResultStatus Spin::onCycleUpdate()
 {
-  rclcpp::Duration time_remaining = end_time_ - steady_clock_.now();
+  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
   if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
     stopRobot();
     RCLCPP_WARN(

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -241,9 +241,6 @@ class PlannerServer : public nav2_util::LifecycleNode
   double max_planner_duration_;
   std::string planner_ids_concat_;
 
-  // Clock
-  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
-
   // TF buffer
   std::shared_ptr<tf2_ros::Buffer> tf_;
 

nav2_planner/src/planner_server.cpp

@@ -362,7 +362,7 @@ void PlannerServer::computePlanThroughPoses()
 {
   std::lock_guard<std::mutex> lock(dynamic_params_lock_);
 
-  auto start_time = steady_clock_.now();
+  auto start_time = this->now();
 
   // Initialize the ComputePathThroughPoses goal and result
   auto goal = action_server_poses_->get_current_goal();
@@ -425,7 +425,7 @@ void PlannerServer::computePlanThroughPoses()
     result->path = concat_path;
     publishPlan(result->path);
 
-    auto cycle_duration = steady_clock_.now() - start_time;
+    auto cycle_duration = this->now() - start_time;
     result->planning_time = cycle_duration;
 
     if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) {
@@ -484,7 +484,7 @@ PlannerServer::computePlan()
 {
   std::lock_guard<std::mutex> lock(dynamic_params_lock_);
 
-  auto start_time = steady_clock_.now();
+  auto start_time = this->now();
 
   // Initialize the ComputePathToPose goal and result
   auto goal = action_server_pose_->get_current_goal();
@@ -521,7 +521,7 @@ PlannerServer::computePlan()
     // Publish the plan for visualization purposes
     publishPlan(result->path);
 
-    auto cycle_duration = steady_clock_.now() - start_time;
+    auto cycle_duration = this->now() - start_time;
     result->planning_time = cycle_duration;
 
     if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) {

nav2_smoother/include/nav2_smoother/nav2_smoother.hpp

@@ -166,8 +166,6 @@ class SmootherServer : public nav2_util::LifecycleNode
   std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
   std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
   std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
-
-  rclcpp::Clock steady_clock_;
 };
 
 }  // namespace nav2_smoother

nav2_smoother/src/nav2_smoother.cpp

@@ -252,7 +252,7 @@ bool SmootherServer::findSmootherId(
 
 void SmootherServer::smoothPlan()
 {
-  auto start_time = steady_clock_.now();
+  auto start_time = this->now();
 
   RCLCPP_INFO(get_logger(), "Received a path to smooth.");
 
@@ -276,7 +276,7 @@ void SmootherServer::smoothPlan()
 
     result->was_completed = smoothers_[current_smoother_]->smooth(
       result->path, goal->max_smoothing_duration);
-    result->smoothing_duration = steady_clock_.now() - start_time;
+    result->smoothing_duration = this->now() - start_time;
 
     if (!result->was_completed) {
       RCLCPP_INFO(

nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp

@@ -35,7 +35,9 @@ BackupBehaviorTester::BackupBehaviorTester()
 : is_active_(false),
   initial_pose_received_(false)
 {
-  node_ = rclcpp::Node::make_shared("backup_behavior_test");
+  rclcpp::NodeOptions options;
+  options.parameter_overrides({{"use_sim_time", true}});
+  node_ = rclcpp::Node::make_shared("backup_behavior_test", options);
 
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py

@@ -88,9 +88,8 @@ def main(argv=sys.argv[1:]):
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
     test1_action = ExecuteProcess(
-        cmd=[testExecutable],
-        name='test_backup_behavior_node',
-        output='screen')
+        cmd=[testExecutable], name='test_backup_behavior_node', output='screen',
+    )
 
     lts = LaunchTestService()
     lts.add_test_action(ld, test1_action)

nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp

@@ -35,7 +35,9 @@ DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester()
 : is_active_(false),
   initial_pose_received_(false)
 {
-  node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test");
+  rclcpp::NodeOptions options;
+  options.parameter_overrides({{"use_sim_time", true}});
+  node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test", options);
 
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py

@@ -88,9 +88,8 @@ def main(argv=sys.argv[1:]):
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
     test1_action = ExecuteProcess(
-        cmd=[testExecutable],
-        name='test_spin_behavior_node',
-        output='screen')
+        cmd=[testExecutable], name='test_spin_behavior_node', output='screen',
+    )
 
     lts = LaunchTestService()
     lts.add_test_action(ld, test1_action)

nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py

@@ -88,9 +88,8 @@ def main(argv=sys.argv[1:]):
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
     test1_action = ExecuteProcess(
-        cmd=[testExecutable],
-        name='test_wait_behavior_node',
-        output='screen')
+        cmd=[testExecutable], name='test_wait_behavior_node', output='screen',
+    )
 
     lts = LaunchTestService()
     lts.add_test_action(ld, test1_action)

nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp

@@ -164,7 +164,6 @@ bool WaitBehaviorTester::behaviorTest(
 
   RCLCPP_INFO(node_->get_logger(), "result received");
 
-
   if ((node_->now() - start_time).seconds() < static_cast<double>(wait_time)) {
     return false;
   }

nav2_velocity_smoother/src/velocity_smoother.cpp

@@ -134,7 +134,7 @@ VelocitySmoother::on_activate(const rclcpp_lifecycle::State &)
   RCLCPP_INFO(get_logger(), "Activating");
   smoothed_cmd_pub_->on_activate();
   double timer_duration_ms = 1000.0 / smoothing_frequency_;
-  timer_ = create_wall_timer(
+  timer_ = this->create_wall_timer(
     std::chrono::milliseconds(static_cast<int>(timer_duration_ms)),
     std::bind(&VelocitySmoother::smootherTimer, this));
 
@@ -342,7 +342,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
         }
 
         double timer_duration_ms = 1000.0 / smoothing_frequency_;
-        timer_ = create_wall_timer(
+        timer_ = this->create_wall_timer(
           std::chrono::milliseconds(static_cast<int>(timer_duration_ms)),
           std::bind(&VelocitySmoother::smootherTimer, this));
       } else if (name == "velocity_timeout") {

nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp

@@ -73,6 +73,7 @@ class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor
   int waypoint_pause_duration_;
   bool is_enabled_;
   rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")};
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace nav2_waypoint_follower

nav2_waypoint_follower/plugins/wait_at_waypoint.cpp

@@ -42,6 +42,7 @@ void WaitAtWaypoint::initialize(
     throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"};
   }
   logger_ = node->get_logger();
+  clock_ = node->get_clock();
   nav2_util::declare_parameter_if_not_declared(
     node,
     plugin_name + ".waypoint_pause_duration",
@@ -77,7 +78,7 @@ bool WaitAtWaypoint::processAtWaypoint(
     logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds",
     curr_waypoint_index,
     waypoint_pause_duration_);
-  rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_));
+  clock_->sleep_for(std::chrono::milliseconds(waypoint_pause_duration_));
   return true;
 }
 }  // namespace nav2_waypoint_follower