fix issue #624 : add TimeoutNode::halt()

Author: facontidavide

CMakeLists.txt

@@ -97,11 +97,12 @@ list(APPEND BT_SOURCE
     src/actions/test_node.cpp
     src/actions/sleep_node.cpp
 
+    src/decorators/delay_node.cpp
     src/decorators/inverter_node.cpp
     src/decorators/repeat_node.cpp
     src/decorators/retry_node.cpp
+    src/decorators/timeout_node.cpp
     src/decorators/subtree_node.cpp
-    src/decorators/delay_node.cpp
 
     src/controls/if_then_else_node.cpp
     src/controls/fallback_node.cpp

include/behaviortree_cpp/decorators/delay_node.h

@@ -1,3 +1,15 @@
+/*  Copyright (C) 2018-2023 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
 #pragma once
 
 #include "behaviortree_cpp/decorator_node.h"
@@ -35,12 +47,8 @@ class DelayNode : public DecoratorNode
   {
     return {InputPort<unsigned>("delay_msec", "Tick the child after a few milliseconds")};
   }
-  void halt() override
-  {
-    delay_started_ = false;
-    timer_.cancelAll();
-    DecoratorNode::halt();
-  }
+
+  void halt() override;
 
 private:
   TimerQueue<> timer_;

include/behaviortree_cpp/decorators/timeout_node.h

@@ -1,3 +1,15 @@
+/*  Copyright (C) 2018-2023 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
 #pragma once
 
 #include "behaviortree_cpp/decorator_node.h"
@@ -8,19 +20,18 @@ namespace BT
 {
 /**
  * @brief The TimeoutNode will halt() a running child if
- * the latter has been RUNNING for more than a give time.
+ * the latter has been RUNNING longer than a given time.
  * The timeout is in milliseconds and it is passed using the port "msec".
  *
- * If timeout is reached it returns FAILURE.
+ * If timeout is reached, the node returns FAILURE.
  *
  * Example:
  *
  * <Timeout msec="5000">
  *    <KeepYourBreath/>
  * </Timeout>
  */
-template <typename _Clock = std::chrono::steady_clock,
-          typename _Duration = std::chrono::steady_clock::duration>
+
 class TimeoutNode : public DecoratorNode
 {
 public:
@@ -56,67 +67,12 @@ class TimeoutNode : public DecoratorNode
   }
 
 private:
-  TimerQueue<_Clock, _Duration> timer_;
-
-  virtual BT::NodeStatus tick() override
-  {
-    if (read_parameter_from_ports_)
-    {
-      if (!getInput("msec", msec_))
-      {
-        throw RuntimeError("Missing parameter [msec] in TimeoutNode");
-      }
-    }
 
-    if (!timeout_started_)
-    {
-      timeout_started_ = true;
-      setStatus(NodeStatus::RUNNING);
-      child_halted_ = false;
+  virtual BT::NodeStatus tick() override;
 
-      if (msec_ > 0)
-      {
-        timer_id_ = timer_.add(std::chrono::milliseconds(msec_), [this](bool aborted) {
-          // Return immediately if the timer was aborted.
-          // This function could be invoked during destruction of this object and
-          // we don't want to access member variables if not needed.
-          if (aborted)
-          {
-            return;
-          }
-          std::unique_lock<std::mutex> lk(timeout_mutex_);
-          if (child()->status() == NodeStatus::RUNNING)
-          {
-            child_halted_ = true;
-            haltChild();
-            emitWakeUpSignal();
-          }
-        });
-      }
-    }
-
-    std::unique_lock<std::mutex> lk(timeout_mutex_);
-
-    if (child_halted_)
-    {
-      timeout_started_ = false;
-      return NodeStatus::FAILURE;
-    }
-    else
-    {
-      const NodeStatus child_status = child()->executeTick();
-      if(isStatusCompleted(child_status))
-      {
-        timeout_started_ = false;
-        timeout_mutex_.unlock();
-        timer_.cancel(timer_id_);
-        timeout_mutex_.lock();
-        resetChild();
-      }
-      return child_status;
-    }
-  }
+  void halt() override;
 
+  TimerQueue<> timer_;
   std::atomic<bool> child_halted_;
   uint64_t timer_id_;
 
@@ -125,4 +81,5 @@ class TimeoutNode : public DecoratorNode
   bool timeout_started_;
   std::mutex timeout_mutex_;
 };
+
 }   // namespace BT

src/bt_factory.cpp

@@ -67,7 +67,7 @@ BehaviorTreeFactory::BehaviorTreeFactory():
   registerNodeType<RetryNode>("RetryUntilSuccessful");
   registerNodeType<KeepRunningUntilFailureNode>("KeepRunningUntilFailure");
   registerNodeType<RepeatNode>("Repeat");
-  registerNodeType<TimeoutNode<>>("Timeout");
+  registerNodeType<TimeoutNode>("Timeout");
   registerNodeType<DelayNode>("Delay");
   registerNodeType<RunOnceNode>("RunOnce");
 

src/decorators/delay_node.cpp

@@ -1,7 +1,7 @@
 /*  Contributed by Indraneel on 26/04/2020
 */
+
 #include "behaviortree_cpp/decorators/delay_node.h"
-#include "behaviortree_cpp/action_node.h"
 
 namespace BT
 {
@@ -23,6 +23,13 @@ DelayNode::DelayNode(const std::string& name, const NodeConfig& config) :
   read_parameter_from_ports_(true)
 {}
 
+void DelayNode::halt()
+{
+  delay_started_ = false;
+  timer_.cancelAll();
+  DecoratorNode::halt();
+}
+
 NodeStatus DelayNode::tick()
 {
   if (read_parameter_from_ports_)

src/decorators/timeout_node.cpp

@@ -0,0 +1,85 @@
+/*  Copyright (C) 2018-2023 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp/decorators/timeout_node.h"
+
+namespace BT
+{
+
+NodeStatus TimeoutNode::tick()
+{
+  if (read_parameter_from_ports_)
+  {
+    if (!getInput("msec", msec_))
+    {
+      throw RuntimeError("Missing parameter [msec] in TimeoutNode");
+    }
+  }
+
+  if (!timeout_started_)
+  {
+    timeout_started_ = true;
+    setStatus(NodeStatus::RUNNING);
+    child_halted_ = false;
+
+    if (msec_ > 0)
+    {
+      timer_id_ = timer_.add(std::chrono::milliseconds(msec_), [this](bool aborted) {
+        // Return immediately if the timer was aborted.
+        // This function could be invoked during destruction of this object and
+        // we don't want to access member variables if not needed.
+        if (aborted)
+        {
+          return;
+        }
+        std::unique_lock<std::mutex> lk(timeout_mutex_);
+        if (child()->status() == NodeStatus::RUNNING)
+        {
+          child_halted_ = true;
+          haltChild();
+          emitWakeUpSignal();
+        }
+      });
+    }
+  }
+
+  std::unique_lock<std::mutex> lk(timeout_mutex_);
+
+  if (child_halted_)
+  {
+    timeout_started_ = false;
+    return NodeStatus::FAILURE;
+  }
+  else
+  {
+    const NodeStatus child_status = child()->executeTick();
+    if(isStatusCompleted(child_status))
+    {
+      timeout_started_ = false;
+      timeout_mutex_.unlock();
+      timer_.cancel(timer_id_);
+      timeout_mutex_.lock();
+      resetChild();
+    }
+    return child_status;
+  }
+}
+
+void TimeoutNode::halt()
+{
+  timeout_started_ = false;
+  timer_.cancelAll();
+  DecoratorNode::halt();
+}
+
+
+}   // namespace BT

tests/gtest_coroutines.cpp

@@ -113,7 +113,7 @@ TEST(CoroTest, do_action_timeout)
   BT::assignDefaultRemapping<SimpleCoroAction>(node_config_);
 
   SimpleCoroAction node(milliseconds(300), false, "Action", node_config_);
-  BT::TimeoutNode<> timeout("TimeoutAction", 200);
+  BT::TimeoutNode timeout("TimeoutAction", 200);
 
   timeout.setChild(&node);
 
@@ -134,7 +134,7 @@ TEST(CoroTest, sequence_child)
 
   SimpleCoroAction actionA(milliseconds(200), false, "action_A", node_config_);
   SimpleCoroAction actionB(milliseconds(200), false, "action_B", node_config_);
-  BT::TimeoutNode<> timeout("timeout", 300);
+  BT::TimeoutNode timeout("timeout", 300);
   BT::SequenceNode sequence("sequence");
 
   timeout.setChild(&sequence);

tests/gtest_decorator.cpp

@@ -20,7 +20,7 @@ using std::chrono::milliseconds;
 
 struct DeadlineTest : testing::Test
 {
-  BT::TimeoutNode<> root;
+  BT::TimeoutNode root;
   BT::AsyncActionTest action;
 
   DeadlineTest() : root("deadline", 300), action("action", milliseconds(500))
@@ -68,7 +68,7 @@ struct RetryTest : testing::Test
 
 struct TimeoutAndRetry : testing::Test
 {
-  BT::TimeoutNode<> timeout_root;
+  BT::TimeoutNode timeout_root;
   BT::RetryNode retry;
   BT::SyncActionTest action;