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