unit test added and problems fixed with onInit()

Davide Faconti · Davide Faconti · commit 18f43ce53b3d · 2018-12-12T10:13:03.000+01:00
diff --git a/CMakeLists.txt b/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)
diff --git a/gtest/gtest_blackboard.cpp b/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 );
+}
diff --git a/gtest/include/action_test_node.h b/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);
diff --git a/include/behaviortree_cpp/action_node.h b/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:
diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h
@@ -137,7 +137,7 @@ class TreeNode
 
     friend class BehaviorTreeFactory;
 
-    void initialize();
+    void initializeOnce();
 
   private:
 
diff --git a/src/action_node.cpp b/src/action_node.cpp
@@ -23,7 +23,7 @@ ActionNodeBase::ActionNodeBase(const std::string& name, const NodeParameters& pa
 
 NodeStatus ActionNodeBase::executeTick()
 {
-    initialize();
+    initializeOnce();
     NodeStatus prev_status = status();
 
     if (prev_status == NodeStatus::IDLE || prev_status == NodeStatus::RUNNING)
@@ -94,7 +94,7 @@ void AsyncActionNode::waitForTick()
 
 NodeStatus AsyncActionNode::executeTick()
 {
-    initialize();
+    initializeOnce();
     //send signal to other thread.
     // The other thread is in charge for changing the status
     if (status() == NodeStatus::IDLE)
@@ -145,7 +145,7 @@ void CoroActionNode::setStatusRunningAndYield()
 
 NodeStatus CoroActionNode::executeTick()
 {
-    initialize();
+    initializeOnce();
     if (status() == NodeStatus::IDLE)
     {
         _p->coro = coroutine::create( [this]() { setStatus(tick()); } );
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
@@ -140,7 +140,7 @@ std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(
     std::unique_ptr<TreeNode> node = it->second(name, params);
     node->setRegistrationName(ID);
     node->setBlackboard(blackboard);
-    node->initialize();
+    node->initializeOnce();
 
     return node;
 }
diff --git a/src/tree_node.cpp b/src/tree_node.cpp
@@ -34,7 +34,7 @@ TreeNode::TreeNode(const std::string& name, const NodeParameters& parameters)
 
 NodeStatus TreeNode::executeTick()
 {
-
+    initializeOnce();
     const NodeStatus status = tick();
     setStatus(status);
     return status;
@@ -115,12 +115,12 @@ void TreeNode::setRegistrationName(const std::string& registration_name)
     registration_name_ = registration_name;
 }
 
-void TreeNode::initialize()
+void TreeNode::initializeOnce()
 {
     if( not_initialized_ )
     {
-        onInit();
         not_initialized_ = false;
+        onInit();
     }
 }
 

Original file line number	Diff line number	Diff line change
`@@ -166,6 +166,7 @@ set(BT_TESTS`
`166`	`166`	`gtest/gtest_fallback.cpp`
`167`	`167`	`gtest/gtest_factory.cpp`
`168`	`168`	`gtest/gtest_decorator.cpp`
	`169`	`+ gtest/gtest_blackboard.cpp`
`169`	`170`	`)`
`170`	`171`
`171`	`172`	`if(ament_cmake_FOUND AND BUILD_TESTING)`
Original file line number	Diff line number	Diff line change
`@@ -33,7 +33,7 @@ class SyncActionTest : public ActionNodeBase`
`33`	`33`	`int tick_count_;`
`34`	`34`	`};`
`35`	`35`
`36`		`-class AsyncActionTest : public ActionNode`
	`36`	`+class AsyncActionTest : public AsyncActionNode`
`37`	`37`	`{`
`38`	`38`	`public:`
`39`	`39`	`AsyncActionTest(const std::string& name);`
Original file line number	Diff line number	Diff line change
`@@ -19,6 +19,14 @@`
`19`	`19`
`20`	`20`	`namespace BT`
`21`	`21`	`{`
	`22`	`+/** IMPORTANT: to avoid unexpected behaviors when Sequence (not SequenceStar) is used`
	`23`	`+ * an Action that returned SUCCESS or FAILURE will not be ticked again unless`
	`24`	`+ * setStatus(IDLE) is called first (reset the Action).`
	`25`	`+ *`
	`26`	`+ * Usually the parent node takes care of this for you.`
	`27`	`+ */`
	`28`	`+`
	`29`	`+`
`22`	`30`	`class ActionNodeBase : public LeafNode`
`23`	`31`	`{`
`24`	`32`	`public:`
Original file line number	Diff line number	Diff line change
`@@ -23,7 +23,7 @@ ActionNodeBase::ActionNodeBase(const std::string& name, const NodeParameters& pa`
`23`	`23`
`24`	`24`	`NodeStatus ActionNodeBase::executeTick()`
`25`	`25`	`{`
`26`		`- initialize();`
	`26`	`+ initializeOnce();`
`27`	`27`	`NodeStatus prev_status = status();`
`28`	`28`
`29`	`29`	`if (prev_status == NodeStatus::IDLE \|\| prev_status == NodeStatus::RUNNING)`
`@@ -94,7 +94,7 @@ void AsyncActionNode::waitForTick()`
`94`	`94`
`95`	`95`	`NodeStatus AsyncActionNode::executeTick()`
`96`	`96`	`{`
`97`		`- initialize();`
	`97`	`+ initializeOnce();`
`98`	`98`	`//send signal to other thread.`
`99`	`99`	`// The other thread is in charge for changing the status`
`100`	`100`	`if (status() == NodeStatus::IDLE)`
`@@ -145,7 +145,7 @@ void CoroActionNode::setStatusRunningAndYield()`
`145`	`145`
`146`	`146`	`NodeStatus CoroActionNode::executeTick()`
`147`	`147`	`{`
`148`		`- initialize();`
	`148`	`+ initializeOnce();`
`149`	`149`	`if (status() == NodeStatus::IDLE)`
`150`	`150`	`{`
`151`	`151`	`_p->coro = coroutine::create( [this]() { setStatus(tick()); } );`
Original file line number	Diff line number	Diff line change
`@@ -140,7 +140,7 @@ std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(`
`140`	`140`	`std::unique_ptr<TreeNode> node = it->second(name, params);`
`141`	`141`	`node->setRegistrationName(ID);`
`142`	`142`	`node->setBlackboard(blackboard);`
`143`		`- node->initialize();`
	`143`	`+ node->initializeOnce();`
`144`	`144`
`145`	`145`	`return node;`
`146`	`146`	`}`
Original file line number	Diff line number	Diff line change
`@@ -34,7 +34,7 @@ TreeNode::TreeNode(const std::string& name, const NodeParameters& parameters)`
`34`	`34`
`35`	`35`	`NodeStatus TreeNode::executeTick()`
`36`	`36`	`{`
`37`		`-`
	`37`	`+ initializeOnce();`
`38`	`38`	`const NodeStatus status = tick();`
`39`	`39`	`setStatus(status);`
`40`	`40`	`return status;`
`@@ -115,12 +115,12 @@ void TreeNode::setRegistrationName(const std::string& registration_name)`
`115`	`115`	`registration_name_ = registration_name;`
`116`	`116`	`}`
`117`	`117`
`118`		`-void TreeNode::initialize()`
	`118`	`+void TreeNode::initializeOnce()`
`119`	`119`	`{`
`120`	`120`	`if( not_initialized_ )`
`121`	`121`	`{`
`122`		`- onInit();`
`123`	`122`	`not_initialized_ = false;`
	`123`	`+ onInit();`
`124`	`124`	`}`
`125`	`125`	`}`
`126`	`126`