From e7457ba8a23a14d1887f29d053a3510ecf082a09 Mon Sep 17 00:00:00 2001 From: Benjamin-Tan Date: Mon, 6 May 2024 11:23:06 +0800 Subject: [PATCH] Cherry-pick from 15c9be0aa5d09dd3be40bb46a66a5175464dd1a4 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 --- .../nav2_behavior_tree/behavior_tree_engine.hpp | 4 +++- nav2_behavior_tree/src/behavior_tree_engine.cpp | 3 ++- .../nav2_behaviors/plugins/drive_on_heading.hpp | 4 ++-- .../include/nav2_behaviors/timed_behavior.hpp | 14 +++++++------- nav2_behaviors/plugins/assisted_teleop.cpp | 8 ++++---- nav2_behaviors/plugins/back_up.cpp | 2 +- nav2_behaviors/plugins/spin.cpp | 4 ++-- .../include/nav2_planner/planner_server.hpp | 3 --- nav2_planner/src/planner_server.cpp | 8 ++++---- .../include/nav2_smoother/nav2_smoother.hpp | 2 -- nav2_smoother/src/nav2_smoother.cpp | 4 ++-- .../behaviors/backup/backup_behavior_tester.cpp | 4 +++- .../backup/test_backup_behavior_launch.py | 5 ++--- .../drive_on_heading_behavior_tester.cpp | 4 +++- .../behaviors/spin/test_spin_behavior_launch.py | 5 ++--- .../behaviors/wait/test_wait_behavior_launch.py | 5 ++--- .../src/behaviors/wait/wait_behavior_tester.cpp | 1 - nav2_velocity_smoother/src/velocity_smoother.cpp | 4 ++-- .../plugins/wait_at_waypoint.hpp | 1 + .../plugins/wait_at_waypoint.cpp | 3 ++- 20 files changed, 44 insertions(+), 44 deletions(-) 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 index 6f0c9bfb91..49bb2efaa4 100644 --- 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 & plugin_libraries); + explicit BehaviorTreeEngine( + const std::vector & plugin_libraries); virtual ~BehaviorTreeEngine() {} /** diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index a5ee96af5e..0a3e100080 100644 --- 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 & plugin_libraries) +BehaviorTreeEngine::BehaviorTreeEngine( + const std::vector & 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 index 7ffba907e5..1f5a0de429 100644 --- 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 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 */ 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 index 579a2fd0ce..2b238eccd4 100644 --- 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,7 +201,7 @@ 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(); @@ -209,7 +209,7 @@ class TimedBehavior : public nav2_core::Behavior 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 index 2882773bc0..fafcb62bae 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -67,7 +67,7 @@ Status AssistedTeleop::onRun(const std::shared_ptrtime_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 index 61246d0509..ec02284200 100644 --- 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 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 index b7df8b3654..a48a86d1df 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -91,14 +91,14 @@ Status Spin::onRun(const std::shared_ptr 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 index 54e740e528..a69d8d199a 100644 --- 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 tf_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 0e393f8304..29957209ec 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -372,7 +372,7 @@ PlannerServer::computePlanThroughPoses() { std::lock_guard 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 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 index 1a4d416503..f23cd05e38 100644 --- 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 costmap_sub_; std::shared_ptr footprint_sub_; std::shared_ptr 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 index 0130edd5a6..795fd1fb2f 100644 --- 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 index a270c8ab08..974c2d60b2 100644 --- 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(node_->get_clock()); tf_listener_ = std::make_shared(*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 index bf7f4f04f1..440e6e49af 100755 --- 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 index 8db6ef6406..967c6beb04 100644 --- 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(node_->get_clock()); tf_listener_ = std::make_shared(*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 index d26795834b..76cdcd1fc1 100755 --- 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 index 37f2ebd34c..112955bec4 100755 --- 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 index ea6e58afb7..dffd52f911 100644 --- 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(wait_time)) { return false; } diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index bc101fd732..cb2ca19916 100644 --- 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(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); @@ -342,7 +342,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(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 index 911ae441a2..cbd2700246 100644 --- 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 index 917655e21a..77e20bcf76 100644 --- 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