Use BT.CPP Tree::sleep (#4761)

* Use BT.cpp sleep Signed-off-by: Tony Najjar <[email protected]> * Implement BT Loop Rate Signed-off-by: Tony Najjar <[email protected]> * Fix formatting Signed-off-by: Tony Najjar <[email protected]> * Fix formatting Signed-off-by: Tony Najjar <[email protected]> * move to nav2_behavior_tree Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix lint Signed-off-by: Tony Najjar <[email protected]> * cache Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]>
ros-navigation · Nov 27, 2024 · dc26fa4 · dc26fa4
1 parent 90a6c8d
commit dc26fa4
Show file tree

Hide file tree

Showing 4 changed files with 89 additions and 5 deletions.
diff --git 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\
                 -<no value>\
@@ -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
@@ -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 <memory>
+
+#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<rclcpp::Clock>(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
@@ -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<bool()> 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
@@ -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) {