diff --git a/.circleci/config.yml b/.circleci/config.yml index 9cf5261e8d..385950c5f0 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v28\ + - "<< parameters.key >>-v29\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v28\ + - "<< parameters.key >>-v29\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v28\ + key: "<< parameters.key >>-v29\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ 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 new file mode 100644 index 0000000000..d7ebd5b4ac --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp @@ -0,0 +1,82 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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_BEHAVIOR_TREE__UTILS__LOOP_RATE_HPP_ +#define NAV2_BEHAVIOR_TREE__UTILS__LOOP_RATE_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" + +namespace nav2_behavior_tree +{ + +class LoopRate +{ +public: + LoopRate(const rclcpp::Duration & period, BT::Tree * tree) + : clock_(std::make_shared(RCL_STEADY_TIME)), period_(period), + last_interval_(clock_->now()), tree_(tree) + {} + + // Similar to rclcpp::WallRate::sleep() but using tree_->sleep() + bool sleep() + { + // Time coming into sleep + auto now = clock_->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (next_interval <= now) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // 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(time_to_sleep_ns); + return true; + } + + std::chrono::nanoseconds period() const + { + return std::chrono::nanoseconds(period_.nanoseconds()); + } + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Duration period_; + rclcpp::Time last_interval_; + BT::Tree * tree_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__UTILS__LOOP_RATE_HPP_ diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 70a192d719..7e8dc0a09c 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/utils/shared_library.h" +#include "nav2_behavior_tree/utils/loop_rate.hpp" namespace nav2_behavior_tree { @@ -49,7 +50,7 @@ BehaviorTreeEngine::run( std::function cancelRequested, std::chrono::milliseconds loopTimeout) { - rclcpp::WallRate loopRate(loopTimeout); + nav2_behavior_tree::LoopRate loopRate(loopTimeout, tree); BT::NodeStatus result = BT::NodeStatus::RUNNING; // Loop until something happens with ROS or the node completes 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 d22ed27c7a..56a66a4602 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 @@ -25,6 +25,7 @@ #include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_behavior_tree/utils/loop_rate.hpp" #include "test_msgs/action/fibonacci.hpp" @@ -260,7 +261,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) BT::NodeStatus result = BT::NodeStatus::RUNNING; // BT loop execution rate - rclcpp::WallRate loopRate(10ms); + nav2_behavior_tree::LoopRate loopRate(10ms, tree_.get()); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {