[TreeNode] add failed_ filed and has_failed()

uml-robotics · Nov 2, 2020 · fdf4ec9 · fdf4ec9
1 parent 2a7e30a
commit fdf4ec9
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git a/include/behaviortree_cpp_v3/tree_node.h b/include/behaviortree_cpp_v3/tree_node.h
@@ -82,6 +82,8 @@ class TreeNode
 
     void setStatus(NodeStatus new_status);
 
+    bool has_failed() const;
+
     /// Name of the instance, not the type
     const std::string& name() const;
 
@@ -152,7 +154,7 @@ class TreeNode
         parent_ = parent;
     }
 
-    TreeNode* getParent() {
+    TreeNode* getParent() const {
         return parent_;
     }
 
@@ -185,6 +187,8 @@ class TreeNode
 
     NodeStatus status_;
 
+    bool failed_ = false;
+
     std::condition_variable state_condition_variable_;
 
     mutable std::mutex state_mutex_;

diff --git a/src/tree_node.cpp b/src/tree_node.cpp
@@ -44,6 +44,10 @@ void TreeNode::setStatus(NodeStatus new_status)
         std::unique_lock<std::mutex> UniqueLock(state_mutex_);
         prev_status = status_;
         status_ = new_status;
+
+        if (status_ == NodeStatus::FAILURE) {
+            this->failed_ = true;
+        }
     }
     if (prev_status != new_status)
     {
@@ -59,6 +63,10 @@ NodeStatus TreeNode::status() const
     return status_;
 }
 
+bool TreeNode::has_failed() const {
+    return failed_;
+}
+
 NodeStatus TreeNode::waitValidStatus()
 {
     std::unique_lock<std::mutex> lock(state_mutex_);