Reactive async

include/behaviortree_cpp/action_node.h

@@ -38,7 +38,7 @@ class ActionNodeBase : public LeafNode
     ActionNodeBase(const std::string& name, const NodeConfiguration& config);
     ~ActionNodeBase() override = default;
 
-    virtual NodeType type() const override final
+    virtual NodeType type() const override
     {
         return NodeType::ACTION;
     }
@@ -114,6 +114,11 @@ class AsyncActionNode : public ActionNodeBase
 
     void stopAndJoinThread();
 
+    virtual NodeType type() const override final
+    {
+        return NodeType::ACTION_ASYNC;
+    }
+
   private:
 
     // The method that will be executed by the thread

include/behaviortree_cpp/basic_types.h

@@ -26,7 +26,8 @@ enum class NodeType
     CONDITION,
     CONTROL,
     DECORATOR,
-    SUBTREE
+    SUBTREE,
+    ACTION_ASYNC
 };
 
 /// Enumerates the states every node can be in after execution during a particular

include/behaviortree_cpp/control_node.h

@@ -50,6 +50,7 @@ class ControlNode : public TreeNode
     {
         return NodeType::CONTROL;
     }
+    void propagateHalt(unsigned i);
 };
 }
 

include/behaviortree_cpp/tree_node.h

@@ -1,4 +1,4 @@
-/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+/* Copyright (C) 2015-2019 Michele Colledanchise -  All Rights Reserved
 *  Copyright (C) 2018-2019 Davide Faconti, Eurecat -  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"),
@@ -21,13 +21,17 @@
 #include "behaviortree_cpp/basic_types.h"
 #include "behaviortree_cpp/blackboard.h"
 #include "behaviortree_cpp/utils/strcat.hpp"
+//#include <behaviortree_cpp/control_node.h>
 
 #ifdef _MSC_VER 
 #pragma warning(disable : 4127) 
 #endif
 
+
 namespace BT
 {
+class ControlNode;
+
 /// This information is used mostly by the XMLParser.
 struct TreeNodeManifest
 {
@@ -147,8 +151,13 @@ class TreeNode
     static StringView stripBlackboardPointer(StringView str);
 
     static Optional<StringView> getRemappedKey(StringView port_name, StringView remapping_value);
+    void set_parent_prt(ControlNode *parent_ptr);
 
-  protected:
+    ControlNode *parent_prt() const;
+    unsigned int child_index() const;
+    void set_child_index(unsigned int child_index);
+
+protected:
     /// Method to be implemented by the user
     virtual BT::NodeStatus tick() = 0;
 
@@ -161,6 +170,9 @@ class TreeNode
     }
 
     void modifyPortsRemapping(const PortsRemapping& new_remapping);
+    unsigned int child_index_;
+
+    ControlNode* parent_prt_ = nullptr;
 
   private:
     const std::string name_;

src/control_node.cpp

@@ -22,6 +22,8 @@ ControlNode::ControlNode(const std::string& name, const NodeConfiguration& confi
 
 void ControlNode::addChild(TreeNode* child)
 {
+    child->set_child_index(children_nodes_.size());
+    child->set_parent_prt(this);
     children_nodes_.push_back(child);
 }
 
@@ -54,4 +56,13 @@ void ControlNode::haltChildren(unsigned i)
     }
 }
 
+void ControlNode::propagateHalt(unsigned i)
+{
+    haltChildren(i + 1);
+    if (parent_prt_ != nullptr)
+    {
+        ((ControlNode*)parent_prt_)->propagateHalt(child_index_);
+    }
+}
+
 } // end namespace

src/controls/reactive_fallback.cpp

@@ -1,5 +1,5 @@
 /* Copyright (C) 2019 Davide Faconti, Eurecat -  All Rights Reserved
-*
+*  Copyright (C) 2019 Michele Colledanchise -  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:
@@ -11,7 +11,7 @@
 */
 
 #include "behaviortree_cpp/controls/reactive_fallback.h"
-
+#include <thread>
 namespace BT
 {
 
@@ -22,31 +22,70 @@ NodeStatus ReactiveFallback::tick()
     for (size_t index = 0; index < childrenCount(); index++)
     {
         TreeNode* current_child_node = children_nodes_[index];
-        const NodeStatus child_status = current_child_node->executeTick();
+        NodeStatus child_status = NodeStatus::IDLE;
 
-        switch (child_status)
+        if (current_child_node->type() != NodeType::ACTION_ASYNC)
         {
-            case NodeStatus::RUNNING:
+            child_status = current_child_node->executeTick();
+        }
+        else
+        {
+            if (current_child_node->status() != NodeStatus::RUNNING)
             {
-                haltChildren(index+1);
-                return NodeStatus::RUNNING;
-            }
+                /* if not running already, tick it. I assume that ACTION_ASYNC returns running for (at least the first tick),
+                hence the halt of possibly other async actions should be sent before ticking current_child_node */
 
-            case NodeStatus::FAILURE:
+                if (parent_prt_ !=  nullptr)
+                {
+                    parent_prt_->propagateHalt(child_index_);
+                }
+
+                current_child_node->executeTick();
+
+                do
+                {
+                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+                    child_status = current_child_node->status();
+
+                } while (child_status == NodeStatus::IDLE);
+            }
+            else
             {
-                failure_count++;
-            }break;
+                child_status = NodeStatus::RUNNING;
+            }
+        }
 
-            case NodeStatus::SUCCESS:
+        switch (child_status)
+        {
+        case NodeStatus::RUNNING:
+        {
+            haltChildren(index+1);
+            return NodeStatus::RUNNING;
+        }break;
+
+        case NodeStatus::FAILURE:
+        {
+            failure_count++;
+            if (current_child_node->type() == NodeType::ACTION_ASYNC)
             {
-                haltChildren(0);
-                return NodeStatus::SUCCESS;
+                current_child_node->setStatus(NodeStatus::IDLE);
             }
+        }break;
 
-            case NodeStatus::IDLE:
+        case NodeStatus::SUCCESS:
+        {
+            haltChildren(index+1);
+            if (current_child_node->type() == NodeType::ACTION_ASYNC)
             {
-                throw LogicError("A child node must never return IDLE");
+                current_child_node->setStatus(NodeStatus::IDLE);
             }
+            return NodeStatus::SUCCESS;
+        }break;
+
+        case NodeStatus::IDLE:
+        {
+            throw LogicError("A child node must never return IDLE");
+        }
         }   // end switch
     } //end for
 
@@ -60,4 +99,3 @@ NodeStatus ReactiveFallback::tick()
 }
 
 }
-

src/controls/reactive_sequence.cpp

@@ -1,5 +1,5 @@
 /* Copyright (C) 2019 Davide Faconti, Eurecat -  All Rights Reserved
-*
+*  Copyright (C) 2019 Michele Colledanchise -  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:
@@ -11,43 +11,81 @@
 */
 
 #include "behaviortree_cpp/controls/reactive_sequence.h"
+#include <thread>
 
 namespace BT
 {
 
 NodeStatus ReactiveSequence::tick()
 {
     size_t success_count = 0;
-    size_t running_count = 0;
 
     for (size_t index = 0; index < childrenCount(); index++)
     {
         TreeNode* current_child_node = children_nodes_[index];
-        const NodeStatus child_status = current_child_node->executeTick();
+        NodeStatus child_status = NodeStatus::IDLE;
 
-        switch (child_status)
+        if (current_child_node->type() != NodeType::ACTION_ASYNC)
+        {
+            child_status = current_child_node->executeTick();
+        }
+        else
         {
-            case NodeStatus::RUNNING:
+            if (current_child_node->status() != NodeStatus::RUNNING)
             {
-                running_count++;
-                haltChildren(index+1);
-                return NodeStatus::RUNNING;
-            }
+                /* if not running already, tick it. I assume that ACTION_ASYNC returns running for (at least the first tick),
+                hence the halt of possibly other async actions should be sent before ticking current_child_node */
 
-            case NodeStatus::FAILURE:
-            {
-                haltChildren(0);
-                return NodeStatus::FAILURE;
+                if (parent_prt_ !=  nullptr)
+                {
+                    parent_prt_->propagateHalt(child_index_);
+                }
+
+                current_child_node->executeTick();
+
+                do
+                {
+                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+                    child_status = current_child_node->status();
+
+                } while (child_status == NodeStatus::IDLE);
             }
-            case NodeStatus::SUCCESS:
+            else
             {
-                success_count++;
-            }break;
+                child_status = NodeStatus::RUNNING;
+            }
+        }
+
+        switch (child_status)
+        {
+        case NodeStatus::RUNNING:
+        {
+            haltChildren(index+1);
+            return NodeStatus::RUNNING;
+        }
 
-            case NodeStatus::IDLE:
+        case NodeStatus::FAILURE:
+        {
+            haltChildren(index+1);
+            if (current_child_node->type() == NodeType::ACTION_ASYNC)
+            {
+                current_child_node->setStatus(NodeStatus::IDLE);
+            }
+            return NodeStatus::FAILURE;
+        }
+        case NodeStatus::SUCCESS:
+        {
+            success_count++;
+            if (current_child_node->type() == NodeType::ACTION_ASYNC)
             {
-                throw LogicError("A child node must never return IDLE");
+                current_child_node->setStatus(NodeStatus::IDLE);
             }
+        }break;
+
+        case NodeStatus::IDLE:
+        {
+            throw LogicError("A child node must never return IDLE");
+        }
         }   // end switch
     } //end for
 
@@ -59,5 +97,4 @@ NodeStatus ReactiveSequence::tick()
     return NodeStatus::RUNNING;
 }
 
-
 }

src/tree_node.cpp

@@ -1,4 +1,4 @@
-/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+/* Copyright (C) 2015-2019 Michele Colledanchise -  All Rights Reserved
  * Copyright (C) 2018-2019 Davide Faconti, Eurecat -  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"),
@@ -13,6 +13,7 @@
 
 #include "behaviortree_cpp/tree_node.h"
 #include <cstring>
+#include <behaviortree_cpp/control_node.h>
 
 namespace BT
 {
@@ -161,4 +162,24 @@ void TreeNode::modifyPortsRemapping(const PortsRemapping &new_remapping)
     }
 }
 
+unsigned int TreeNode::child_index() const
+{
+    return child_index_;
+}
+
+void TreeNode::set_child_index(unsigned int child_index)
+{
+    child_index_ = child_index;
+}
+
+void TreeNode::set_parent_prt(ControlNode *parent_ptr)
+{
+    parent_prt_ = parent_ptr;
+}
+
+ControlNode *TreeNode::parent_prt() const
+{
+    return parent_prt_;
+}
+
 }   // end namespace

tests/CMakeLists.txt

@@ -16,6 +16,10 @@ set(BT_TESTS
   gtest_coroutines.cpp
   navigation_test.cpp
   gtest_subtree.cpp
+  gtest_reactive_sequence.cpp
+  gtest_reactive_fallback.cpp
+  gtest_reactive_tree.cpp
+
 )
 
 if(ament_cmake_FOUND AND BUILD_TESTING)