From ca4f07413b20cd1c8b9b318c6851c88239fc1ceb Mon Sep 17 00:00:00 2001
From: Sara <sara.al-arab@logivations.com>
Date: Mon, 26 Sep 2022 10:36:27 +0300
Subject: [PATCH] add new suspendNode

---
 CMakeLists.txt                                |  2 +
 include/behaviortree_cpp_v3/behavior_tree.h   |  2 +
 .../decorators/suspend_node.h                 | 66 ++++++++++++++++
 src/bt_factory.cpp                            |  2 +
 src/decorators/suspend_node.cpp               | 76 +++++++++++++++++++
 5 files changed, 148 insertions(+)
 create mode 100644 include/behaviortree_cpp_v3/decorators/suspend_node.h
 create mode 100644 src/decorators/suspend_node.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6bdda94..83d7c19 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -159,6 +159,8 @@ list(APPEND BT_SOURCE
     src/decorators/keep_trying_node.cpp
     src/decorators/subtree_node.cpp
     src/decorators/delay_node.cpp
+    src/decorators/suspend_node.cpp
+
 
     src/controls/if_then_else_node.cpp
     src/controls/fallback_node.cpp
diff --git a/include/behaviortree_cpp_v3/behavior_tree.h b/include/behaviortree_cpp_v3/behavior_tree.h
index 9ab299f..0c20d80 100644
--- a/include/behaviortree_cpp_v3/behavior_tree.h
+++ b/include/behaviortree_cpp_v3/behavior_tree.h
@@ -44,6 +44,8 @@
 #include "behaviortree_cpp_v3/decorators/blackboard_precondition.h"
 #include "behaviortree_cpp_v3/decorators/timeout_node.h"
 #include "behaviortree_cpp_v3/decorators/delay_node.h"
+#include "behaviortree_cpp_v3/decorators/suspend_node.h"
+
 
 #include <iostream>
 
diff --git a/include/behaviortree_cpp_v3/decorators/suspend_node.h b/include/behaviortree_cpp_v3/decorators/suspend_node.h
new file mode 100644
index 0000000..ab0b51f
--- /dev/null
+++ b/include/behaviortree_cpp_v3/decorators/suspend_node.h
@@ -0,0 +1,66 @@
+#ifndef DECORATOR_DELAY_NODE_H
+#define DECORATOR_DELAY_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+#include <atomic>
+#include "timer_queue.h"
+
+namespace BT
+{
+/**
+ * @brief The delay node will introduce a delay of a few milliseconds
+ * and then tick the child returning the status of the child as it is 
+ * upon completion
+ * The delay is in milliseconds and it is passed using the port "delay_msec".
+ *
+ * During the delay the node changes status to RUNNING
+ *
+ * Example:
+ *
+ * <Delay delay_msec="5000">
+ *    <KeepYourBreath/>
+ * </Delay>
+ */
+class SuspendNode : public DecoratorNode
+{
+  public:
+    SuspendNode(const std::string& name, unsigned milliseconds);
+
+    SuspendNode(const std::string& name, const NodeConfiguration& config);
+
+    ~SuspendNode() override
+    {
+        halt();
+    }
+
+    static PortsList providedPorts()
+    {
+        return {InputPort<unsigned>("delay_msec", "Tick the child after a few milliseconds")};
+    }
+    void halt() override
+    {
+
+        delay_started_ = false;
+        delay_complete_=false;
+        delay_aborted_=false;
+        timer_.cancelAll();
+        DecoratorNode::halt();
+    }
+
+  private:
+    TimerQueue<> timer_;
+    uint64_t timer_id_;
+
+    virtual BT::NodeStatus tick() override;
+
+    bool delay_started_;
+    bool delay_complete_;
+    bool delay_aborted_;
+    unsigned msec_;
+    bool read_parameter_from_ports_;
+    std::mutex delay_mutex_;
+};
+
+}   // namespace BT
+
+#endif   // DELAY_NODE_H
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
index 4c03b51..a5d77d5 100644
--- a/src/bt_factory.cpp
+++ b/src/bt_factory.cpp
@@ -42,6 +42,8 @@ BehaviorTreeFactory::BehaviorTreeFactory()
     registerNodeType<RepeatNode>("Repeat");
     registerNodeType<TimeoutNode<>>("Timeout");
     registerNodeType<DelayNode>("Delay");
+    registerNodeType<SuspendNode>("Suspend");
+
 
     registerNodeType<ForceSuccessNode>("ForceSuccess");
     registerNodeType<ForceFailureNode>("ForceFailure");
diff --git a/src/decorators/suspend_node.cpp b/src/decorators/suspend_node.cpp
new file mode 100644
index 0000000..1f128d7
--- /dev/null
+++ b/src/decorators/suspend_node.cpp
@@ -0,0 +1,76 @@
+/*  Contributed by Indraneel on 26/04/2020
+*/
+#include "behaviortree_cpp_v3/decorators/suspend_node.h"
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace BT
+{
+SuspendNode::SuspendNode(const std::string& name, unsigned milliseconds)
+  : DecoratorNode(name, {})
+  , delay_started_(false)
+  , delay_aborted_(false)
+  , msec_(milliseconds)
+  , read_parameter_from_ports_(false)
+{
+    setRegistrationID("Delay");
+}
+
+SuspendNode::SuspendNode(const std::string& name, const NodeConfiguration& config)
+  : DecoratorNode(name, config)
+  , delay_started_(false)
+  , delay_aborted_(false)
+  , msec_(0)
+  , read_parameter_from_ports_(true)
+{
+}
+
+NodeStatus SuspendNode::tick()
+{
+    if (read_parameter_from_ports_)
+    {
+        if (!getInput("delay_msec", msec_))
+        {
+            throw RuntimeError("Missing parameter [delay_msec] in SuspendNode");
+        }
+    }
+
+    if (!delay_started_)
+    {
+        delay_complete_ = false;
+        delay_aborted_ = false;
+        delay_started_ = true;
+        setStatus(NodeStatus::RUNNING);
+
+        timer_id_ = timer_.add(std::chrono::milliseconds(msec_),
+                               [this](bool aborted)
+        {
+            std::unique_lock<std::mutex> lk(delay_mutex_);
+            if (!aborted)
+            {
+                delay_complete_ = true;
+            }
+            else
+            {
+                delay_aborted_ = true;
+            }
+        });
+    }
+
+    std::unique_lock<std::mutex> lk(delay_mutex_);
+
+    if (delay_aborted_)
+    {
+        return NodeStatus::FAILURE;
+    }
+    else if (delay_complete_)
+    {
+        auto child_status = child()->executeTick();
+        return child_status;
+    }
+    else
+    {
+        return NodeStatus::RUNNING;
+    }
+}
+
+}   // namespace BT