Merge pull request microsoft#25 from facontidavide/devel

Preliminary changes to add tracing/logging
dempo93 · May 30, 2018 · 22f6258 · 22f6258
2 parents 45b0c16 + e06aa6d
commit 22f6258
Show file tree

Hide file tree

Showing 33 changed files with 3,253 additions and 228 deletions.
diff --git a/3rdparty/flatbuffers/flatbuffers.h b/3rdparty/flatbuffers/flatbuffers.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -100,8 +100,10 @@ endif()
 # COMPILING EXAMPLES
 ######################################################
 
-#TODO
 
+add_executable(crossdoor_example gtest/crossdoor_example.cpp )
+target_link_libraries(crossdoor_example
+    behavior_tree_core )
 
 ######################################################
 # INSTALLATION OF LIBRARY AND EXECUTABLE

diff --git a/gtest/crossdoor_example.cpp b/gtest/crossdoor_example.cpp
@@ -0,0 +1,63 @@
+#include "crossdoor_dummy_nodes.h"
+#include "behavior_tree_core/xml_parsing.h"
+#include "behavior_tree_logger/bt_cout_logger.h"
+
+// clang-format off
+
+const std::string xml_text = R"(
+
+                             <root main_tree_to_execute = "MainTree" >
+
+                             <BehaviorTree ID="MainTree">
+                             <Fallback name="root_selector">
+
+                             <Sequence name="door_open_sequence">
+                             <Action ID="IsDoorOpen" />
+                             <Action ID="PassThroughDoor" />
+                             </Sequence>
+
+                             <Sequence name="door_closed_sequence">
+                             <Decorator ID="Negation">
+                             <Action ID="IsDoorOpen" />
+                             </Decorator>
+                             <Action ID="OpenDoor" />
+                             <Action ID="PassThroughDoor" />
+                             <Action ID="CloseDoor" />
+                             </Sequence>
+
+                             <Action ID="PassThroughWindow" />
+
+                             </Fallback>
+                             </BehaviorTree>
+
+                             </root>
+                             )";
+
+// clang-format on
+
+int main(int argc, char** argv)
+{
+    using namespace BT;
+
+    BT::BehaviorTreeFactory factory;
+
+    // register all the actions into the factory
+    CrossDoor cross_door(factory);
+
+    XMLParser parser;
+    parser.loadFromText(xml_text);
+
+    std::vector<BT::TreeNodePtr> nodes;
+    BT::TreeNodePtr root_node = parser.instantiateTree(factory, nodes);
+
+    StdCoutLogger logger(root_node.get());
+
+    cross_door.CloseDoor();
+
+    std::cout << "---------------" << std::endl;
+    root_node->executeTick();
+    std::cout << "---------------" << std::endl;
+    root_node->executeTick();
+    std::cout << "---------------" << std::endl;
+    return 0;
+}
diff --git a/gtest/include/crossdoor_dummy_nodes.h b/gtest/include/crossdoor_dummy_nodes.h
@@ -1,5 +1,7 @@
 #include "behavior_tree_core/bt_factory.h"
 
+using namespace BT;
+
 class CrossDoor
 {
   public:
@@ -19,48 +21,48 @@ class CrossDoor
 
     BT::NodeStatus IsDoorOpen()
     {
-        return door_open_ ? BT::SUCCESS : BT::FAILURE;
+        return door_open_ ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
     }
 
     BT::NodeStatus IsDoorLocked()
     {
-        return door_locked_ ? BT::SUCCESS : BT::FAILURE;
+        return door_locked_ ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
     }
 
     BT::NodeStatus UnlockDoor()
     {
         door_locked_ = false;
-        return BT::SUCCESS;
+        return NodeStatus::SUCCESS;
     }
 
     BT::NodeStatus PassThroughDoor()
     {
-        return door_open_ ? BT::SUCCESS : BT::FAILURE;
+        return door_open_ ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
     }
 
     BT::NodeStatus PassThroughWindow()
     {
-        return BT::SUCCESS;
+        return NodeStatus::SUCCESS;
     }
 
     BT::NodeStatus OpenDoor()
     {
         if (door_locked_)
-            return BT::FAILURE;
+            return NodeStatus::FAILURE;
         door_open_ = true;
-        return BT::SUCCESS;
+        return NodeStatus::SUCCESS;
     }
 
     BT::NodeStatus CloseDoor()
     {
         if (door_open_)
         {
             door_open_ = false;
-            return BT::SUCCESS;
+            return NodeStatus::SUCCESS;
         }
         else
         {
-            return BT::FAILURE;
+            return NodeStatus::FAILURE;
         }
     }
 

diff --git a/gtest/src/action_test_node.cpp b/gtest/src/action_test_node.cpp
@@ -32,34 +32,23 @@ BT::NodeStatus BT::ActionTestNode::tick()
     int i = 0;
     while ( !stop_loop_ && i++ < time_)
     {
-        DEBUG_STDOUT(" Action " << name() << "running! Thread id:" << std::this_thread::get_id());
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
 
     if ( !stop_loop_ )
     {
-        if (boolean_value_)
-        {
-            DEBUG_STDOUT(" Action " << name() << " Done!");
-            return BT::SUCCESS;
-        }
-        else
-        {
-            DEBUG_STDOUT(" Action " << name() << " FAILED!");
-            return BT::FAILURE;
-        }
+        return boolean_value_ ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
     }
     else
     {
-        return BT::IDLE;
+        return NodeStatus::IDLE;
     }
 }
 
 void BT::ActionTestNode::halt()
 {
     stop_loop_ = true;
-    setStatus(BT::IDLE);
-    DEBUG_STDOUT("HALTED state set!");
+    setStatus(NodeStatus::IDLE);
 }
 
 void BT::ActionTestNode::set_time(int time)

diff --git a/gtest/src/condition_test_node.cpp b/gtest/src/condition_test_node.cpp
@@ -28,13 +28,13 @@ BT::NodeStatus BT::ConditionTestNode::tick()
 
     if (boolean_value_)
     {
-        setStatus(BT::SUCCESS);
-        return BT::SUCCESS;
+        setStatus(NodeStatus::SUCCESS);
+        return NodeStatus::SUCCESS;
     }
     else
     {
-        setStatus(BT::FAILURE);
-        return BT::FAILURE;
+        setStatus(NodeStatus::FAILURE);
+        return NodeStatus::FAILURE;
     }
 }
 

diff --git a/include/behavior_tree_core/action_node.h b/include/behavior_tree_core/action_node.h
@@ -28,7 +28,7 @@ class ActionNodeBase : public LeafNode
 
     virtual NodeType type() const override final
     {
-        return ACTION_NODE;
+        return NodeType::ACTION;
     }
 };
 
@@ -94,13 +94,13 @@ class ActionNode : public ActionNodeBase
     // This method MUST to be overriden by the user.
     virtual NodeStatus tick() override
     {
-        return BT::IDLE;
+        return NodeStatus::IDLE;
     }
 
     // This method MUST to be overriden by the user.
     virtual void halt() override
     {
-        setStatus(BT::IDLE);
+        setStatus(NodeStatus::IDLE);
     }
 
     void stopAndJoinThread();

diff --git a/include/behavior_tree_core/behavior_tree.h b/include/behavior_tree_core/behavior_tree.h
@@ -31,12 +31,52 @@
 
 namespace BT
 {
-void recursiveVisitor(const TreeNode *node, const std::function<void(const TreeNode*)> visitor);
+void recursiveVisitor(TreeNode *node, std::function<void(TreeNode *)> visitor);
 
 /**
  * Debug function to print on a stream
  */
 void printTreeRecursively(const TreeNode* root_node);
+
+
+/**
+ * @brief buildSerializedStatusSnapshot can be used to create a serialize buffer that can be stored
+ * (or sent to a client application) to know the status of all the nodes of a tree.
+ * It is not "human readable".
+ *
+ * @param root_node
+ * @param serialized_buffer
+ */
+
+typedef std::vector<std::pair<uint16_t,uint8_t>> SerializedTreeStatus;
+
+void buildSerializedStatusSnapshot(const TreeNode *root_node, SerializedTreeStatus& serialized_buffer);
+
+
+/**
+ * @brief toStr converts NodeStatus to string. Optionally colored.
+ */
+const char* toStr(const BT::NodeStatus& status, bool colored = false);
+
+
+inline std::ostream& operator<<(std::ostream& os, const BT::NodeStatus& status)
+{
+    os << toStr(status);
+    return os;
+}
+
+/**
+ * @brief toStr converts NodeType to string.
+ */
+const char* toStr(const BT::NodeType& type);
+
+inline std::ostream& operator<<(std::ostream& os, const BT::NodeType& type)
+{
+    os << toStr(type);
+    return os;
+}
+
+
 }
 
 #endif   // BEHAVIOR_TREE_H
diff --git a/include/behavior_tree_core/condition_node.h b/include/behavior_tree_core/condition_node.h
@@ -30,7 +30,7 @@ class ConditionNode : public LeafNode
 
     virtual NodeType type() const override final
     {
-        return CONDITION_NODE;
+        return NodeType::CONDITION;
     }
 };
 }

diff --git a/include/behavior_tree_core/control_node.h b/include/behavior_tree_core/control_node.h
@@ -50,7 +50,7 @@ class ControlNode : public TreeNode
 
     virtual NodeType type() const override final
     {
-        return CONTROL_NODE;
+        return NodeType::CONTROL;
     }
 };
 }

diff --git a/include/behavior_tree_core/decorator_node.h b/include/behavior_tree_core/decorator_node.h
@@ -28,7 +28,7 @@ class DecoratorNode : public TreeNode
 
     virtual NodeType type() const override final
     {
-        return DECORATOR_NODE;
+        return NodeType::DECORATOR;
     }
 };
 

diff --git a/include/behavior_tree_core/decorator_subtree_node.h b/include/behavior_tree_core/decorator_subtree_node.h
@@ -19,9 +19,9 @@ class DecoratorSubtreeNode : public DecoratorNode
     virtual BT::NodeStatus tick()
     {
         NodeStatus prev_status = status();
-        if ( prev_status== BT::IDLE)
+        if ( prev_status== NodeStatus::IDLE)
         {
-            setStatus(BT::RUNNING);
+            setStatus(NodeStatus::RUNNING);
         }
         auto status = child_node_->executeTick();
         setStatus(status);
-Original file line number
+Diff line change
@@ Expand Up / @@ -30,7 +30,7 @@ class ConditionNode : public LeafNode @@
         virtual NodeType type() const override final
         {
-            return CONDITION_NODE;
+            return NodeType::CONDITION;
         }
     };
     }
@@ Expand Down @@