diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index a1b37d62bb1..6d6ae171d6d 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -23,9 +23,9 @@ #include "nav2_amcl/amcl_node.hpp" #include +#include #include #include -#include #include #include #include @@ -148,7 +148,7 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) // Create pose save timer if save_pose_rate > 0 if (save_pose_rate_ > 0.0) { - save_pose_timer_ = create_wall_timer( + save_pose_timer_ = this->create_timer( std::chrono::duration(1.0 / save_pose_rate_), std::bind(&AmclNode::savePoseTimerCallback, this)); } 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 d7f16f2a87e..b94fbe59e45 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 @@ -141,8 +141,8 @@ class BehaviorTreeEngine // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; - // Clock - rclcpp::Clock::SharedPtr clock_; + // Node handle used to obtain clocks at run time + nav2::LifecycleNode::WeakPtr node_; // Groot2 monitor std::unique_ptr groot_monitor_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp index 728855f9807..4e981962471 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp @@ -16,6 +16,7 @@ #define NAV2_BEHAVIOR_TREE__UTILS__LOOP_RATE_HPP_ #include +#include #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/bt_factory.h" @@ -27,8 +28,10 @@ namespace nav2_behavior_tree class LoopRate { public: - LoopRate(const rclcpp::Duration & period, BT::Tree * tree) - : clock_(std::make_shared(RCL_STEADY_TIME)), period_(period), + LoopRate( + const rclcpp::Duration & period, BT::Tree * tree, + rclcpp::Clock::SharedPtr clock) + : clock_(std::move(clock)), period_(period), last_interval_(clock_->now()), tree_(tree) {} @@ -57,11 +60,16 @@ class LoopRate // Either way do not sleep and return false return false; } - // Calculate the time to sleep - auto time_to_sleep = next_interval - now; - std::chrono::nanoseconds time_to_sleep_ns(time_to_sleep.nanoseconds()); - // Sleep (can get interrupted by emitWakeUpSignal()) - tree_->sleep(std::chrono::duration_cast(time_to_sleep_ns)); + auto wake_up = tree_->wakeUpSignal(); + static constexpr auto poll_interval = std::chrono::milliseconds(10); + while (clock_->now() < next_interval) { + // Sleep using the wake-up signal directly so we can poll the target clock. + // tree_->sleep() always waits in wall-clock time, which diverges from sim + // time. Instead we do short wall-clock waits and re-check our clock. + if (wake_up->waitFor(poll_interval)) { // Preempted by emitWakeUpSignal() + return true; + } + } return true; } diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 31b5ab3e776..0318add3326 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -26,6 +26,7 @@ #include "behaviortree_cpp/utils/shared_library.h" #include "nav2_behavior_tree/json_utils.hpp" #include "nav2_behavior_tree/utils/loop_rate.hpp" +#include "nav2_ros_common/rate.hpp" namespace nav2_behavior_tree { @@ -39,8 +40,7 @@ BehaviorTreeEngine::BehaviorTreeEngine( factory_.registerFromPlugin(loader.getOSName(p)); } - // clock for throttled debug log - clock_ = node->get_clock(); + node_ = node; } BtStatus @@ -50,7 +50,16 @@ BehaviorTreeEngine::run( std::function cancelRequested, std::chrono::milliseconds loopTimeout) { - nav2_behavior_tree::LoopRate loopRate(loopTimeout, tree); + auto node = node_.lock(); + if (!node) { + RCLCPP_ERROR( + rclcpp::get_logger("BehaviorTreeEngine"), + "BehaviorTreeEngine node expired. Exiting with failure."); + return BtStatus::FAILED; + } + + auto rate_clock = nav2::selectClock(node); + nav2_behavior_tree::LoopRate loopRate(loopTimeout, tree, rate_clock); BT::NodeStatus result = BT::NodeStatus::RUNNING; // Loop until something happens with ROS or the node completes @@ -70,7 +79,7 @@ BehaviorTreeEngine::run( if (!loopRate.sleep()) { RCLCPP_DEBUG_THROTTLE( rclcpp::get_logger("BehaviorTreeEngine"), - *clock_, 1000, + *rate_clock, 1000, "Behavior Tree tick rate %0.2f was exceeded!", 1.0 / (loopRate.period().count() * 1.0e-9)); } diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index d44cf7c73af..b1a123e4830 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -267,7 +267,8 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) BT::NodeStatus result = BT::NodeStatus::RUNNING; // BT loop execution rate - nav2_behavior_tree::LoopRate loopRate(10ms, tree_.get()); + nav2_behavior_tree::LoopRate loopRate( + 10ms, tree_.get(), std::make_shared(RCL_STEADY_TIME)); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 82b1d3d07c9..8120980a475 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -32,6 +32,7 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_util/twist_publisher.hpp" #include "nav2_ros_common/simple_action_server.hpp" +#include "nav2_ros_common/rate.hpp" #include "nav2_core/behavior.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" @@ -226,7 +227,7 @@ class TimedBehavior : public nav2_core::Behavior } auto start_time = clock_->now(); - rclcpp::WallRate loop_rate(cycle_frequency_); + nav2::Rate loop_rate(node_.lock(), cycle_frequency_); while (rclcpp::ok()) { elapsed_time_ = clock_->now() - start_time; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 04a03573c8f..8794ee04339 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -22,6 +22,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "nav2_core/controller_exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_ros_common/rate.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/path_utils.hpp" #include "nav2_controller/controller_server.hpp" @@ -494,7 +495,7 @@ void ControllerServer::computeControl() } last_valid_cmd_time_ = now(); - rclcpp::WallRate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); while (rclcpp::ok()) { auto start_time = this->now(); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index b1eeb7c939e..4ef7ea3096f 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -47,6 +47,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_util/execution_timer.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_ros_common/rate.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/create_timer_ros.hpp" #include "nav2_util/robot_utils.hpp" @@ -532,7 +533,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) RCLCPP_DEBUG(get_logger(), "Entering loop"); - rclcpp::WallRate r(frequency); // 200ms by default + nav2::Rate r(this, frequency); // 200ms by default while (rclcpp::ok() && !map_update_thread_shutdown_) { nav2_util::ExecutionTimer timer; diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 39a70e2bafb..7825f6a8a3f 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "angles/angles.h" +#include "nav2_ros_common/rate.hpp" #include "opennav_docking/docking_server.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/utils.hpp" @@ -175,7 +176,7 @@ void DockingServer::dockRobot() { std::lock_guard lock_reinit(param_handler_->getMutex()); action_start_time_ = this->now(); - rclcpp::Rate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); auto goal = docking_action_server_->get_current_goal(); auto result = std::make_shared(); @@ -393,7 +394,7 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta throw opennav_docking_core::FailedToDetectDock("Failed to start the detection process."); } - rclcpp::Rate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(params_->initial_perception_timeout); while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) { @@ -419,7 +420,7 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( tf2::getYaw(target_pose.pose.orientation) + M_PI); - rclcpp::Rate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(params_->rotate_to_dock_timeout); @@ -452,7 +453,7 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po bool DockingServer::approachDock( Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward) { - rclcpp::Rate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(params_->dock_approach_timeout); @@ -522,7 +523,7 @@ bool DockingServer::waitForCharge(Dock * dock) return true; } - rclcpp::Rate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(params_->wait_charge_timeout); while (rclcpp::ok()) { @@ -550,7 +551,7 @@ bool DockingServer::waitForCharge(Dock * dock) bool DockingServer::resetApproach( const geometry_msgs::msg::PoseStamped & staging_pose, bool backward) { - rclcpp::Rate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(params_->dock_approach_timeout); while (rclcpp::ok()) { @@ -621,7 +622,7 @@ void DockingServer::undockRobot() { std::lock_guard lock_reinit(param_handler_->getMutex()); action_start_time_ = this->now(); - rclcpp::Rate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); auto goal = undocking_action_server_->get_current_goal(); auto result = std::make_shared(); diff --git a/nav2_following/opennav_following/src/following_server.cpp b/nav2_following/opennav_following/src/following_server.cpp index 7bf26e0c522..c06cda46553 100644 --- a/nav2_following/opennav_following/src/following_server.cpp +++ b/nav2_following/opennav_following/src/following_server.cpp @@ -14,6 +14,7 @@ // limitations under the License. #include "angles/angles.h" +#include "nav2_ros_common/rate.hpp" #include "opennav_docking_core/docking_exceptions.hpp" #include "opennav_following/following_server.hpp" #include "nav2_util/geometry_utils.hpp" @@ -180,7 +181,7 @@ void FollowingServer::followObject() { std::lock_guard lock_reinit(param_handler_->getMutex()); action_start_time_ = this->now(); - rclcpp::Rate loop_rate(params_->controller_frequency); + nav2::Rate loop_rate(this, params_->controller_frequency); auto goal = following_action_server_->get_current_goal(); auto result = std::make_shared(); @@ -525,7 +526,7 @@ bool FollowingServer::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) if (detected.header.stamp == rclcpp::Time(0)) { auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(params_->detection_timeout); - rclcpp::Rate wait_rate(params_->controller_frequency); + nav2::Rate wait_rate(this, params_->controller_frequency); while (this->now() - start < timeout) { // Check if a new detection arrived if (detected_dynamic_pose_.header.stamp != rclcpp::Time(0)) { diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 63f2f01e0a9..a1caa469372 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -21,6 +21,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "nav2_ros_common/interface_factories.hpp" using namespace std::chrono_literals; using namespace std::placeholders; @@ -73,7 +74,8 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] = std::string("Shutting down "); - init_timer_ = this->create_wall_timer( + init_timer_ = nav2::create_timer( + this, 0s, [this]() -> void { init_timer_->cancel(); @@ -81,7 +83,8 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) createLifecycleServiceClients(); createLifecycleServiceServers(); if (autostart_) { - init_timer_ = this->create_wall_timer( + init_timer_ = nav2::create_timer( + this, 0s, [this]() -> void { init_timer_->cancel(); @@ -481,7 +484,8 @@ LifecycleManager::createBondTimer() } message("Creating bond timer..."); - bond_timer_ = this->create_wall_timer( + bond_timer_ = nav2::create_timer( + this, 200ms, std::bind(&LifecycleManager::checkBondConnections, this), callback_group_); @@ -556,7 +560,8 @@ LifecycleManager::checkBondConnections() // Initialize the bond respawn timer to check if server comes back online // after a failure, within a maximum timeout period. if (attempt_respawn_reconnection_) { - bond_respawn_timer_ = this->create_wall_timer( + bond_respawn_timer_ = nav2::create_timer( + this, 1s, std::bind(&LifecycleManager::checkBondRespawnConnection, this), callback_group_); diff --git a/nav2_lifecycle_manager/test/test_bond.cpp b/nav2_lifecycle_manager/test/test_bond.cpp index b2dbc10115f..4020605e6cf 100644 --- a/nav2_lifecycle_manager/test/test_bond.cpp +++ b/nav2_lifecycle_manager/test/test_bond.cpp @@ -19,6 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/node_thread.hpp" +#include "nav2_ros_common/rate.hpp" #include "nav2_lifecycle_manager/lifecycle_manager.hpp" #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" @@ -128,7 +129,7 @@ class TestFixture TEST(LifecycleBondTest, POSITIVE) { // let the lifecycle server come up - rclcpp::Rate(1).sleep(); + nav2::WallRate(1).sleep(); auto node = std::make_shared("lifecycle_manager_test_service_client"); nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node); @@ -140,14 +141,14 @@ TEST(LifecycleBondTest, POSITIVE) EXPECT_TRUE(client.startup()); // check if bond is connected after being activated - rclcpp::Rate(5).sleep(); + nav2::WallRate(5).sleep(); EXPECT_TRUE(bond_tester->isBondConnected()); EXPECT_EQ(bond_tester->getState(), "activated"); bond_tester->breakBond(); // bond should be disconnected now and lifecycle manager should know and react to reset - rclcpp::Rate(5).sleep(); + nav2::WallRate(5).sleep(); EXPECT_EQ( nav2_lifecycle_manager::SystemStatus::INACTIVE, client.is_active(std::chrono::nanoseconds(1000000000))); diff --git a/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp index ad43a75ac67..d61f789387a 100644 --- a/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp +++ b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp @@ -19,6 +19,7 @@ #include #include "nav2_ros_common/qos_profiles.hpp" +#include "nav2_ros_common/rate.hpp" #include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -57,7 +58,7 @@ TEST(MapSaverCLI, CLITest) publisher->publish(std::move(msg)); - rclcpp::Rate(1).sleep(); + nav2::WallRate(1).sleep(); // succeed on real map RCLCPP_INFO(node->get_logger(), "Calling saver..."); @@ -70,7 +71,7 @@ TEST(MapSaverCLI, CLITest) auto return_code = system(command.c_str()); EXPECT_EQ(return_code, 0); - rclcpp::Rate(0.25).sleep(); + nav2::WallRate(0.25).sleep(); RCLCPP_INFO(node->get_logger(), "Checking on file..."); @@ -97,7 +98,7 @@ TEST(MapSaverCLI, CLITest) return_code = system(command.c_str()); EXPECT_EQ(return_code, 256); - rclcpp::Rate(0.25).sleep(); + nav2::WallRate(0.25).sleep(); RCLCPP_INFO(node->get_logger(), "Checking on file..."); @@ -110,7 +111,7 @@ TEST(MapSaverCLI, CLITest) return_code = system(command.c_str()); EXPECT_EQ(return_code, 0); - rclcpp::Rate(0.5).sleep(); + nav2::WallRate(0.5).sleep(); RCLCPP_INFO(node->get_logger(), "Testing invalid mode..."); command = @@ -119,7 +120,7 @@ TEST(MapSaverCLI, CLITest) return_code = system(command.c_str()); EXPECT_EQ(return_code, 0); - rclcpp::Rate(0.5).sleep(); + nav2::WallRate(0.5).sleep(); RCLCPP_INFO(node->get_logger(), "Testing missing argument..."); command = @@ -128,7 +129,7 @@ TEST(MapSaverCLI, CLITest) return_code = system(command.c_str()); EXPECT_EQ(return_code, 65280); - rclcpp::Rate(0.5).sleep(); + nav2::WallRate(0.5).sleep(); RCLCPP_INFO(node->get_logger(), "Testing wrong argument..."); command = @@ -137,7 +138,7 @@ TEST(MapSaverCLI, CLITest) return_code = system(command.c_str()); EXPECT_EQ(return_code, 65280); - rclcpp::Rate(0.5).sleep(); + nav2::WallRate(0.5).sleep(); command = std::string( diff --git a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp index 3f9f9db5131..875d6ef3cb4 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -27,6 +27,7 @@ #include "nav2_ros_common/subscription.hpp" #include "nav2_ros_common/action_client.hpp" #include "rclcpp_action/client.hpp" +#include "nav2_ros_common/rate.hpp" namespace nav2 { @@ -331,6 +332,53 @@ typename nav2::ActionClient::SharedPtr create_action_client( } // namespace interfaces +/** + * @brief A sim-time-aware timer creator for Nav2. + * + * When use_sim_time is true, the timer uses the node's ROS clock (simulation time). + * When use_sim_time is false, a steady (monotonic) clock is used. + * + * Usage: + * auto timer = nav2::create_timer(this, 50ms, callback); + */ +template +rclcpp::TimerBase::SharedPtr create_timer( + NodeT node, + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return rclcpp::create_timer( + node, + selectClock(node), + rclcpp::Duration(period), + std::move(callback), + group); +} + +/** + * @brief A steady-clock wall timer creator for Nav2. + * + * This timer always uses RCL_STEADY_TIME and is never sim-time-aware. + * + * Usage: + * auto timer = nav2::create_wall_timer(this, 50ms, callback); + */ +template +rclcpp::TimerBase::SharedPtr create_wall_timer( + NodeT node, + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return rclcpp::create_timer( + node, + std::make_shared(RCL_STEADY_TIME), + rclcpp::Duration(period), + std::move(callback), + group); +} + } // namespace nav2 #endif // NAV2_ROS_COMMON__INTERFACE_FACTORIES_HPP_ diff --git a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp index 3e67feec660..98db3383160 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -31,6 +31,7 @@ #include "bondcpp/bond.hpp" #include "bond/msg/constants.hpp" #include "nav2_ros_common/interface_factories.hpp" +#include "nav2_ros_common/rate.hpp" namespace nav2 { @@ -297,13 +298,51 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode return nav2::CallbackReturn::SUCCESS; } + /** + * @brief Create a sim-time-aware timer for Nav2 lifecycle nodes. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr) + { + return rclcpp::create_timer( + nav2::selectClock(this), + period, + std::move(callback), + group, + this->get_node_base_interface().get(), + this->get_node_timers_interface().get()); + } + + /** + * @brief Create a steady-clock wall timer for Nav2 lifecycle nodes. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr) + { + return rclcpp::create_timer( + std::make_shared(RCL_STEADY_TIME), + period, + std::move(callback), + group, + this->get_node_base_interface().get(), + this->get_node_timers_interface().get()); + } + /** * @brief Automatically configure and active the node */ void autostart() { using lifecycle_msgs::msg::State; - autostart_timer_ = this->create_wall_timer( + autostart_timer_ = this->create_timer( 0s, [this]() -> void { autostart_timer_->cancel(); diff --git a/nav2_ros_common/include/nav2_ros_common/rate.hpp b/nav2_ros_common/include/nav2_ros_common/rate.hpp new file mode 100644 index 00000000000..690ffb0bed3 --- /dev/null +++ b/nav2_ros_common/include/nav2_ros_common/rate.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2026 Dexory +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROS_COMMON__RATE_HPP_ +#define NAV2_ROS_COMMON__RATE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rate.hpp" +#include "rclcpp/create_timer.hpp" + +namespace nav2 +{ + +/** + * @brief Select the appropriate clock for rate-limiting or timers. + * + * When use_sim_time is true, returns the node's ROS clock (simulation time). + * When use_sim_time is false, returns a steady (monotonic) clock to avoid + * issues with system clock jumps (e.g. NTP corrections). + * + * @param node Any ROS node pointer or shared_ptr + * @return The appropriate clock + */ +template +rclcpp::Clock::SharedPtr selectClock(NodeT node) +{ + bool use_sim_time = false; + auto params = node->get_node_parameters_interface(); + if (params->has_parameter("use_sim_time")) { + use_sim_time = params->get_parameter("use_sim_time").as_bool(); + } + if (use_sim_time) { + return node->get_clock(); + } + return std::make_shared(RCL_STEADY_TIME); +} + +/** + * @brief A sim-time-aware rate for Nav2 loops. + * + * When use_sim_time is true, the rate is governed by the node's ROS clock + * (simulation time). When use_sim_time is false, a steady (monotonic) clock + * is used to avoid issues with system clock jumps (e.g. NTP corrections). + */ +class Rate : public rclcpp::Rate +{ +public: + /** + * @brief Construct a Rate from a node and frequency. + * @param node Any ROS node pointer or shared_ptr (used to check use_sim_time and obtain clock) + * @param rate Desired frequency in Hz + */ + template + explicit Rate(NodeT node, double rate) + : rclcpp::Rate(rate, selectClock(node)) + {} +}; + +/** + * @brief A steady-clock Nav2 wall rate. + * + * This rate always uses RCL_STEADY_TIME and is never sim-time-aware. + */ +class WallRate : public rclcpp::WallRate +{ +public: + explicit WallRate(double rate) + : rclcpp::WallRate(rate) + {} +}; + +} // namespace nav2 + +#endif // NAV2_ROS_COMMON__RATE_HPP_ diff --git a/nav2_route/include/nav2_route/route_tracker.hpp b/nav2_route/include/nav2_route/route_tracker.hpp index a2a3d4e897c..90e999c83fb 100644 --- a/nav2_route/include/nav2_route/route_tracker.hpp +++ b/nav2_route/include/nav2_route/route_tracker.hpp @@ -124,6 +124,7 @@ class RouteTracker rclcpp::Logger logger_{rclcpp::get_logger("RouteTracker")}; double radius_threshold_, boundary_radius_threshold_, tracker_update_rate_; bool aggregate_blocked_ids_; + nav2::LifecycleNode::WeakPtr node_; typename ActionServerTrack::SharedPtr action_server_; std::unique_ptr operations_manager_; std::shared_ptr tf_buffer_; diff --git a/nav2_route/src/route_tracker.cpp b/nav2_route/src/route_tracker.cpp index 6521dc2641b..07a41cc0e31 100644 --- a/nav2_route/src/route_tracker.cpp +++ b/nav2_route/src/route_tracker.cpp @@ -14,6 +14,8 @@ #include "nav2_route/route_tracker.hpp" +#include "nav2_ros_common/rate.hpp" + namespace nav2_route { @@ -25,6 +27,7 @@ void RouteTracker::configure( const std::string & route_frame, const std::string & base_frame) { + node_ = node; clock_ = node->get_clock(); logger_ = node->get_logger(); route_frame_ = route_frame; @@ -163,7 +166,12 @@ TrackerResult RouteTracker::trackRoute( publishFeedback(true, route.start_node->nodeid, 0, 0, {}); } - rclcpp::Rate r(tracker_update_rate_); + auto node = node_.lock(); + if (!node) { + throw nav2_core::RouteException("Route tracker node expired"); + } + + nav2::Rate r(node, tracker_update_rate_); while (rclcpp::ok()) { bool status_change = false, completed = false; diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp index 1f9d9433111..ad105fd68e3 100644 --- a/nav2_route/test/test_graph_loader.cpp +++ b/nav2_route/test/test_graph_loader.cpp @@ -19,6 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_ros_common/rate.hpp" #include "nav2_route/graph_loader.hpp" #include "tf2_ros/static_transform_broadcaster.hpp" @@ -107,7 +108,7 @@ TEST(GraphLoader, test_transformation_api) transform.transform.rotation.w = 1.0; transform.transform.translation.x = 1.0; tf_broadcaster->sendTransform(transform); - rclcpp::Rate(1).sleep(); + nav2::WallRate(1).sleep(); tf_broadcaster->sendTransform(transform); executor.spin_all(std::chrono::milliseconds(50)); diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp index f888a652644..fc707662b7d 100644 --- a/nav2_route/test/test_graph_saver.cpp +++ b/nav2_route/test/test_graph_saver.cpp @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_ros_common/rate.hpp" #include "nav2_route/graph_loader.hpp" #include "nav2_route/graph_saver.hpp" #include "tf2_ros/static_transform_broadcaster.hpp" @@ -143,7 +144,7 @@ TEST(GraphSaver, test_transformation_api) transform.transform.rotation.w = 1.0; transform.transform.translation.x = 1.0; tf_broadcaster->sendTransform(transform); - rclcpp::Rate(1).sleep(); + nav2::WallRate(1).sleep(); tf_broadcaster->sendTransform(transform); executor.spin_all(std::chrono::milliseconds(50)); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 71dd2294253..ba046d47eb0 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -167,7 +167,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_ = this->create_wall_timer( + timer_ = this->create_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); @@ -591,7 +591,7 @@ void VelocitySmoother::updateParametersCallback(const std::vectorcreate_wall_timer( + timer_ = this->create_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); } else if (param_name == "velocity_timeout") { diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp index d5ecafcdc7b..a28567b17a0 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp @@ -76,6 +76,7 @@ class InputAtWaypoint : public nav2_core::WaypointTaskExecutor rclcpp::Duration timeout_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; rclcpp::Clock::SharedPtr clock_; + nav2::LifecycleNode::WeakPtr node_; std::mutex mutex_; nav2::Subscription::SharedPtr subscription_; }; diff --git a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp index e7325717ca7..a12d850e4f8 100644 --- a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp @@ -19,6 +19,7 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_ros_common/rate.hpp" namespace nav2_waypoint_follower { @@ -46,6 +47,7 @@ void InputAtWaypoint::initialize( throw std::runtime_error{"Failed to lock node in input at waypoint plugin!"}; } + node_ = parent; logger_ = node->get_logger(); clock_ = node->get_clock(); @@ -81,8 +83,14 @@ bool InputAtWaypoint::processAtWaypoint( input_received_ = false; + auto node = node_.lock(); + if (!node) { + RCLCPP_ERROR(logger_, "Failed to lock node in input at waypoint plugin."); + return false; + } + rclcpp::Time start = clock_->now(); - rclcpp::Rate r(50); + nav2::Rate r(node, 50); bool input_received = false; while (clock_->now() - start < timeout_) { { diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 4d991178b05..c512f6eff27 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -21,6 +21,8 @@ #include #include +#include "nav2_ros_common/rate.hpp" + namespace nav2_waypoint_follower { @@ -198,7 +200,7 @@ void WaypointFollower::followWaypointsHandler( return; } - rclcpp::WallRate r(params_->loop_rate); + nav2::Rate r(this, params_->loop_rate); // get the goal index, by default, the first in the list of waypoints given. uint32_t goal_index = goal->goal_index; diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp index 9f8af9f8f53..aed99149078 100644 --- a/nav2_waypoint_follower/test/test_task_executors.cpp +++ b/nav2_waypoint_follower/test/test_task_executors.cpp @@ -23,6 +23,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/rate.hpp" #include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" #include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp" #include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp" @@ -65,7 +66,7 @@ TEST(WaypointFollowerTest, InputAtWaypoint) auto publish_message = [&]() -> void { - rclcpp::Rate(5).sleep(); + nav2::WallRate(5).sleep(); auto msg = std::make_unique(); pub->publish(std::move(msg)); executor.spin_some(); @@ -113,7 +114,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) auto publish_message = [&]() -> void { - rclcpp::Rate(5).sleep(); + nav2::WallRate(5).sleep(); auto msg = std::make_unique(); // fill image msg data. msg->encoding = "rgb8"; diff --git a/tools/underlay.repos b/tools/underlay.repos index 7dcb5ae9b69..4691de716e2 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,8 +1,8 @@ repositories: - # BehaviorTree/BehaviorTree.CPP: - # type: git - # url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - # version: 4.8.2 + BehaviorTree/BehaviorTree.CPP: + type: git + url: https://github.com/BehaviorTree/BehaviorTree.CPP.git + version: 7119df95a17086f55e3690cf8d7b7663b59319d0 # ros2/rviz: # type: git # url: https://github.com/ros2/rviz.git