ros-navigation · SteveMacenski · May 8, 2024 · May 6, 2024
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