Merge pull request microsoft#37 from BehaviorTree/issue_33

Merging branch related to issue microsoft#33: add TreeNode::onInit

CMakeLists.txt

@@ -166,6 +166,7 @@ set(BT_TESTS
   gtest/gtest_fallback.cpp
   gtest/gtest_factory.cpp
   gtest/gtest_decorator.cpp
+  gtest/gtest_blackboard.cpp
 )
 
 if(ament_cmake_FOUND AND BUILD_TESTING)

gtest/gtest_blackboard.cpp

@@ -0,0 +1,82 @@
+/* Copyright (C) 2018 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 <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/blackboard/blackboard_local.h"
+
+using namespace BT;
+
+class InitTestNode: public ActionNodeBase
+{
+  public:
+    InitTestNode(bool try_to_access_bb, const std::string& name):
+      ActionNodeBase(name),
+      _value(0)
+    {
+        if( try_to_access_bb )
+        {
+            // this should throw
+            blackboard()->set(KEY(), 33);
+        }
+    }
+
+    void onInit() {
+        blackboard()->get(KEY(), _value);
+    }
+    void halt() {}
+
+    NodeStatus tick()
+    {
+        _value *= 2;
+        blackboard()->set(KEY(), _value);
+        return NodeStatus::SUCCESS;
+    }
+
+    static const char* KEY() { return "my_entry"; }
+
+  private:
+    int _value;
+};
+
+
+
+
+/****************TESTS START HERE***************************/
+
+TEST(BlackboardTest, CheckOInit)
+{
+    auto bb = Blackboard::create<BlackboardLocal>();
+    const auto KEY = InitTestNode::KEY();
+
+    EXPECT_THROW( InitTestNode(true,"init_test"), std::logic_error );
+
+    InitTestNode node(false,"init_test");
+    node.setBlackboard(bb);
+
+    bb->set(KEY, 11 );
+
+    // this should read and write "my_entry", respectively in onInit() and tick()
+    node.executeTick();
+
+    ASSERT_EQ( bb->get<int>(KEY), 22 );
+
+    // check that onInit is executed only once
+    bb->set(KEY, 1 );
+
+    // since this value is read in OnInit(), the node will not notice the change in bb
+    node.setStatus( NodeStatus::IDLE );
+    node.executeTick();
+    ASSERT_EQ( bb->get<int>(KEY), 44 );
+}

gtest/gtest_factory.cpp

@@ -87,7 +87,7 @@ TEST(BehaviorTreeFactory, VerifyLargeTree)
 
     std::vector<BT::TreeNode::Ptr> nodes;
 
-    BT::TreeNode::Ptr root_node = parser.instantiateTree(nodes);
+    BT::TreeNode::Ptr root_node = parser.instantiateTree(nodes, Blackboard::Ptr());
 
     BT::printTreeRecursively(root_node.get());
 
@@ -133,7 +133,7 @@ TEST(BehaviorTreeFactory, Subtree)
 
     std::vector<BT::TreeNode::Ptr> nodes;
 
-    BT::TreeNode::Ptr root_node = parser.instantiateTree(nodes);
+    BT::TreeNode::Ptr root_node = parser.instantiateTree(nodes, Blackboard::Ptr());
     BT::printTreeRecursively(root_node.get());
 
     ASSERT_EQ(root_node->name(), "root_selector");

gtest/include/action_test_node.h

@@ -33,7 +33,7 @@ class SyncActionTest : public ActionNodeBase
     int tick_count_;
 };
 
-class AsyncActionTest : public ActionNode
+class AsyncActionTest : public AsyncActionNode
 {
   public:
     AsyncActionTest(const std::string& name);

include/behaviortree_cpp/action_node.h

@@ -19,6 +19,14 @@
 
 namespace BT
 {
+/** IMPORTANT: to avoid unexpected behaviors when Sequence (not SequenceStar) is used
+ * an Action that returned SUCCESS or FAILURE will not be ticked again unless
+ * setStatus(IDLE) is called first (reset the Action).
+ *
+ * Usually the parent node takes care of this for you.
+ */
+
+
 class ActionNodeBase : public LeafNode
 {
   public:

include/behaviortree_cpp/bt_factory.h

@@ -84,7 +84,8 @@ class BehaviorTreeFactory
      * @return         new node.
      */
     std::unique_ptr<TreeNode> instantiateTreeNode(const std::string& ID, const std::string& name,
-                                                  const NodeParameters& params) const;
+                                                  const NodeParameters& params,
+                                                  const Blackboard::Ptr& blackboard) const;
 
     /** registerNodeType is the method to use to register your custom TreeNode.
      *

include/behaviortree_cpp/tree_node.h

@@ -38,6 +38,14 @@ typedef std::chrono::high_resolution_clock::duration Duration;
 // Abstract base class for Behavior Tree Nodes
 class TreeNode
 {
+
+  private:
+
+    /// This calback will be executed only ONCE after the constructor of the node,
+    /// before the very first tick.
+    /// Override if necessary.
+    virtual void onInit() {}
+
   public:
     /**
      * @brief TreeNode main constructor.
@@ -103,77 +111,22 @@ class TreeNode
     /// creation of the TreeNode instance.
     const NodeParameters& initializationParameters() const;
 
-    /** Get a parameter from the passed NodeParameters and convert it to type T.
+    /** Get a parameter from the NodeParameters and convert it to type T.
      */
     template <typename T>
     BT::optional<T> getParam(const std::string& key) const
     {
         T out;
-        if (getParam(key, out))
-        {
-            return std::move(out);
-        }
-        else
-        {
-            return BT::nullopt;
-        }
+        return getParam(key, out) ? std::move(out) : BT::nullopt;
     }
 
     /** Get a parameter from the passed NodeParameters and convert it to type T.
-     *
-     * return false either if there is no parameter with this key or if conversion failed.
+     *  Return false either if there is no parameter with this key or if conversion failed.
      */
     template <typename T>
-    bool getParam(const std::string& key, T& destination) const
-    {
-        auto it = parameters_.find(key);
-        if (it == parameters_.end())
-        {
-            return false;
-        }
-        const std::string& str = it->second;
+    bool getParam(const std::string& key, T& destination) const;
 
-        try
-        {
-            bool bb_pattern = isBlackboardPattern(str);
-            if( bb_pattern && just_constructed_)
-            {
-                 std::cerr << "You are calling getParam inside a constructor, but this is not allowed "
-                              "when the parameter contains a blackboard.\n"
-                              "You should call getParam inside your tick() method"<< std::endl;
-                 std::logic_error("Calling getParam inside a constructor");
-            }
-            // check if it follows this ${pattern}, if it does, search inside the blackboard
-            if ( bb_ && bb_pattern)
-            {
-                const std::string stripped_key(&str[2], str.size() - 3);
-                const SafeAny::Any* val = bb_->getAny(stripped_key);
-
-                if( val )
-                {
-                    if( std::is_same<T,std::string>::value == false &&
-                        (val->type() == typeid (std::string) ||
-                         val->type() == typeid (SafeAny::SimpleString)))
-                    {
-                        destination = convertFromString<T>(val->cast<std::string>());
-                    }
-                    else{
-                        destination = val->cast<T>();
-                    }
-                }
-                return val != nullptr;
-            }
-            else{
-                destination = convertFromString<T>(str.c_str());
-                return true;
-            }
-        }
-        catch (std::runtime_error& err)
-        {
-            std::cout << "Exception at getParam(" << key << "): " << err.what() << std::endl;
-            return false;
-        }
-    }
+    static bool isBlackboardPattern(StringView str);
 
   protected:
     /// Method to be implemented by the user
@@ -184,9 +137,12 @@ class TreeNode
 
     friend class BehaviorTreeFactory;
 
-    bool just_constructed_;
+    void initializeOnce();
 
   private:
+
+    bool not_initialized_;
+
     const std::string name_;
 
     NodeStatus status_;
@@ -205,10 +161,63 @@ class TreeNode
 
     Blackboard::Ptr bb_;
 
-
-protected:
-    static bool isBlackboardPattern(const std::string& str );
 };
+
+//-------------------------------------------------------
+
+
+template <typename T> inline
+bool TreeNode::getParam(const std::string& key, T& destination) const
+{
+    auto it = parameters_.find(key);
+    if (it == parameters_.end())
+    {
+        return false;
+    }
+    const std::string& str = it->second;
+
+    try
+    {
+        bool bb_pattern = isBlackboardPattern(str);
+        if( bb_pattern && not_initialized_)
+        {
+             std::cerr << "you are calling getParam inside a constructor, but this is not allowed "
+                          "when the parameter contains a blackboard.\n"
+                          "You should call getParam inside your tick() method"<< std::endl;
+             std::logic_error("Calling getParam inside a constructor");
+        }
+        // check if it follows this ${pattern}, if it does, search inside the blackboard
+        if ( bb_pattern && blackboard() )
+        {
+            const std::string stripped_key(&str[2], str.size() - 3);
+            const SafeAny::Any* val = blackboard()->getAny(stripped_key);
+            if( val )
+            {
+                if( std::is_same<T,std::string>::value == false &&
+                    (val->type() == typeid (std::string) ||
+                     val->type() == typeid (SafeAny::SimpleString)))
+                {
+                    destination = convertFromString<T>(val->cast<std::string>());
+                }
+                else{
+                    destination = val->cast<T>();
+                }
+            }
+            return val != nullptr;
+        }
+        else{
+            destination = convertFromString<T>(str.c_str());
+            return true;
+        }
+    }
+    catch (std::runtime_error& err)
+    {
+        std::cout << "Exception at getParam(" << key << "): " << err.what() << std::endl;
+        return false;
+    }
+}
+
+
 }
 
 #endif

include/behaviortree_cpp/xml_parsing.h

@@ -22,7 +22,7 @@ class XMLParser
     using NodeBuilder = std::function<TreeNode::Ptr(const std::string&, const std::string&,
                                                     const NodeParameters&, TreeNode::Ptr)>;
 
-    TreeNode::Ptr instantiateTree(std::vector<TreeNode::Ptr>& nodes);
+    TreeNode::Ptr instantiateTree(std::vector<TreeNode::Ptr>& nodes, const Blackboard::Ptr &blackboard);
 
   private:
 

src/action_node.cpp

@@ -23,7 +23,7 @@ ActionNodeBase::ActionNodeBase(const std::string& name, const NodeParameters& pa
 
 NodeStatus ActionNodeBase::executeTick()
 {
-    just_constructed_ = false;
+    initializeOnce();
     NodeStatus prev_status = status();
 
     if (prev_status == NodeStatus::IDLE || prev_status == NodeStatus::RUNNING)
@@ -94,8 +94,7 @@ void AsyncActionNode::waitForTick()
 
 NodeStatus AsyncActionNode::executeTick()
 {
-    just_constructed_ = false;
-
+    initializeOnce();
     //send signal to other thread.
     // The other thread is in charge for changing the status
     if (status() == NodeStatus::IDLE)
@@ -146,6 +145,7 @@ void CoroActionNode::setStatusRunningAndYield()
 
 NodeStatus CoroActionNode::executeTick()
 {
+    initializeOnce();
     if (status() == NodeStatus::IDLE)
     {
         _p->coro = coroutine::create( [this]() { setStatus(tick()); } );

src/bt_factory.cpp

@@ -123,7 +123,9 @@ void BehaviorTreeFactory::registerFromPlugin(const std::string file_path)
 }
 
 std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(
-    const std::string& ID, const std::string& name, const NodeParameters& params) const
+        const std::string& ID, const std::string& name,
+        const NodeParameters& params,
+        const Blackboard::Ptr& blackboard) const
 {
     auto it = builders_.find(ID);
     if (it == builders_.end())
@@ -137,6 +139,9 @@ std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(
     }
     std::unique_ptr<TreeNode> node = it->second(name, params);
     node->setRegistrationName(ID);
+    node->setBlackboard(blackboard);
+    node->initializeOnce();
+
     return node;
 }