add Sleep Node

Author: facontidavide

CMakeLists.txt

@@ -101,6 +101,7 @@ list(APPEND BT_SOURCE
     src/xml_parsing.cpp
 
     src/actions/test_node.cpp
+    src/actions/sleep_node.cpp
 
     src/decorators/inverter_node.cpp
     src/decorators/repeat_node.cpp

include/behaviortree_cpp/actions/sleep_node.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/utils/timer_queue.h"
+#include <atomic>
+
+namespace BT
+{
+/**
+ * @brief Sleep for a certain amount of time.
+ * Consider also using the decorator <Delay/>
+ *
+ * <Sleep msec="5000"/>
+ */
+class SleepNode : public StatefulActionNode
+{
+public:
+
+  SleepNode(const std::string& name, const NodeConfig& config);
+
+  ~SleepNode() override
+  {
+    halt();
+  }
+
+  NodeStatus onStart() override;
+
+  NodeStatus onRunning() override;
+
+  void onHalted() override;
+
+  static PortsList providedPorts()
+  {
+    return {InputPort<unsigned>("msec")};
+  }
+
+private:
+  TimerQueue<> timer_;
+  uint64_t timer_id_;
+
+  bool timer_waiting_;
+  std::mutex delay_mutex_;
+};
+
+}   // namespace BT

include/behaviortree_cpp/actions/test_node.h

@@ -14,7 +14,7 @@
 #pragma once
 
 #include "behaviortree_cpp/action_node.h"
-#include "behaviortree_cpp/decorators/timer_queue.h"
+#include "behaviortree_cpp/utils/timer_queue.h"
 #include "behaviortree_cpp/scripting/script_parser.hpp"
 
 namespace BT

include/behaviortree_cpp/behavior_tree.h

@@ -38,6 +38,7 @@
 #include "behaviortree_cpp/actions/script_node.h"
 #include "behaviortree_cpp/actions/set_blackboard_node.h"
 #include "behaviortree_cpp/actions/test_node.h"
+#include "behaviortree_cpp/actions/sleep_node.h"
 
 #include "behaviortree_cpp/decorators/force_success_node.h"
 #include "behaviortree_cpp/decorators/force_failure_node.h"

include/behaviortree_cpp/decorators/delay_node.h

@@ -1,8 +1,8 @@
 #pragma once
 
 #include "behaviortree_cpp/decorator_node.h"
+#include "behaviortree_cpp/utils/timer_queue.h"
 #include <atomic>
-#include "timer_queue.h"
 
 namespace BT
 {

include/behaviortree_cpp/decorators/timeout_node.h

@@ -1,8 +1,8 @@
 #pragma once
 
 #include "behaviortree_cpp/decorator_node.h"
+#include "behaviortree_cpp/utils/timer_queue.h"
 #include <atomic>
-#include "behaviortree_cpp/decorators/timer_queue.h"
 
 namespace BT
 {

include/behaviortree_cpp/decorators/timer_queue.h renamed to include/behaviortree_cpp/utils/timer_queue.h

@@ -0,0 +1,51 @@
+#include "behaviortree_cpp/actions/sleep_node.h"
+
+namespace BT
+{
+
+SleepNode::SleepNode(const std::string& name,
+                     const NodeConfig& config) :
+  StatefulActionNode(name, config),
+  timer_waiting_(false)
+{}
+
+NodeStatus SleepNode::onStart()
+{
+  unsigned msec = 0;
+  if (!getInput("msec", msec))
+  {
+    throw RuntimeError("Missing parameter [msec] in SleepNode");
+  }
+
+  if(msec <= 0) {
+    return NodeStatus::SUCCESS;
+  }
+
+  setStatus(NodeStatus::RUNNING);
+
+  timer_waiting_ = true;
+
+  timer_id_ = timer_.add(std::chrono::milliseconds(msec), [this](bool aborted) {
+    std::unique_lock<std::mutex> lk(delay_mutex_);
+    if (!aborted)
+    {
+      emitWakeUpSignal();
+    }
+    timer_waiting_ = false;
+  });
+
+  return NodeStatus::RUNNING;
+}
+
+NodeStatus SleepNode::onRunning()
+{
+  return timer_waiting_ ? NodeStatus::RUNNING : NodeStatus::SUCCESS;
+}
+
+void SleepNode::onHalted()
+{
+  timer_waiting_ = false;
+  timer_.cancel(timer_id_);
+}
+
+}   // namespace BT

src/actions/sleep_node.cpp

@@ -65,6 +65,7 @@ BehaviorTreeFactory::BehaviorTreeFactory()
   registerNodeType<AlwaysFailureNode>("AlwaysFailure");
   registerNodeType<ScriptNode>("Script");
   registerNodeType<SetBlackboard>("SetBlackboard");
+  registerNodeType<SleepNode>("Sleep");
 
   registerNodeType<SubTreeNode>("SubTree");