Cherry-pick from 15c9be0 (#4317)

Benjamin-Tan · web-flow · commit 96d5646a8a50 · 2024-05-08T08:22:37.000-07:00
Convert all wall timers and wall rates to ROS clock respecting rates and timers (#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
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/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() {}
 
   /**
diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/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) {
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/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>
    */
   Status onCycleUpdate() override
   {
-    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(
diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
@@ -108,8 +108,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());
 
@@ -175,7 +175,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")};
@@ -201,15 +201,15 @@ class TimedBehavior : public nav2_core::Behavior
       return;
     }
 
-    auto start_time = steady_clock_.now();
+    auto start_time = clock_->now();
 
     // Initialize the ActionT result
     auto result = std::make_shared<typename ActionT::Result>();
 
     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();
@@ -226,7 +226,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;
@@ -237,14 +237,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;
           action_server_->terminate_current(result);
           onActionCompletion();
           return;
diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp
@@ -67,7 +67,7 @@ Status AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAction::G
 {
   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 Status::SUCCEEDED;
 }
 
@@ -82,7 +82,7 @@ Status 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(
@@ -124,7 +124,7 @@ Status 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;
@@ -134,7 +134,7 @@ Status 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_;
diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp
@@ -31,7 +31,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
   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_,
diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp
@@ -91,14 +91,14 @@ Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> 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 Status::SUCCEEDED;
 }
 
 Status 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(
diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -237,9 +237,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_;
 
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
@@ -372,7 +372,7 @@ PlannerServer::computePlanThroughPoses()
 {
   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_poses_->get_current_goal();
@@ -438,7 +438,7 @@ 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_) {
@@ -464,7 +464,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();
@@ -500,7 +500,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_) {
diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp
@@ -158,8 +158,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
diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
@@ -254,7 +254,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.");
 
@@ -274,7 +274,7 @@ void SmootherServer::smoothPlan()
     result->path = goal->path;
     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(
diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp b/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_);
diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/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)
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp b/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_);
diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/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)
diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/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)
diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp b/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;
   }
diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/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") {
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/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
diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/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

Original file line number	Diff line number	Diff line change
`@@ -25,7 +25,8 @@`
`25`	`25`	`namespace nav2_behavior_tree`
`26`	`26`	`{`
`27`	`27`
`28`		`-BehaviorTreeEngine::BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries)`
	`28`	`+BehaviorTreeEngine::BehaviorTreeEngine(`
	`29`	`+ const std::vector<std::string> & plugin_libraries)`
`29`	`30`	`{`
`30`	`31`	`BT::SharedLibrary loader;`
`31`	`32`	`for (const auto & p : plugin_libraries) {`
Original file line number	Diff line number	Diff line change
`@@ -91,14 +91,14 @@ Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)`
`91`	`91`	`cmd_yaw_);`
`92`	`92`
`93`	`93`	`command_time_allowance_ = command->time_allowance;`
`94`		`- end_time_ = steady_clock_.now() + command_time_allowance_;`
	`94`	`+ end_time_ = this->clock_->now() + command_time_allowance_;`
`95`	`95`
`96`	`96`	`return Status::SUCCEEDED;`
`97`	`97`	`}`
`98`	`98`
`99`	`99`	`Status Spin::onCycleUpdate()`
`100`	`100`	`{`
`101`		`- rclcpp::Duration time_remaining = end_time_ - steady_clock_.now();`
	`101`	`+ rclcpp::Duration time_remaining = end_time_ - this->clock_->now();`
`102`	`102`	`if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {`
`103`	`103`	`stopRobot();`
`104`	`104`	`RCLCPP_WARN(`