quick and dirty solution

Author: miccol

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,6 +21,7 @@
 #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) 
@@ -147,8 +148,13 @@ class TreeNode
     static StringView stripBlackboardPointer(StringView str);
 
     static Optional<StringView> getRemappedKey(StringView port_name, StringView remapping_value);
+    void set_parent_prt(TreeNode *parent_ptr);
 
-  protected:
+    TreeNode* 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 +167,9 @@ class TreeNode
     }
 
     void modifyPortsRemapping(const PortsRemapping& new_remapping);
+    unsigned int child_index_;
+
+    TreeNode* parent_prt_ = nullptr;
 
   private:
     const std::string name_;
@@ -178,6 +187,8 @@ class TreeNode
     NodeConfiguration config_;
 
     std::string registration_ID_;
+
+
 };
 
 //-------------------------------------------------------

src/control_node.cpp

@@ -22,9 +22,14 @@ 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);
 }
 
+
+
+
 unsigned ControlNode::childrenCount() const
 {
     return unsigned(children_nodes_.size());
@@ -54,4 +59,16 @@ void ControlNode::haltChildren(unsigned i)
     }
 }
 
+void ControlNode::propagateHalt(unsigned i)
+{
+//    std::cout << name() << " received a request to propagate halt from index " << i + 1 << std::endl;
+
+    haltChildren(i + 1);
+    if (parent_prt_ != nullptr)
+    {
+        ((ControlNode*)parent_prt_)->propagateHalt(child_index_);
+    }
+}
+
+
 } // end namespace

src/controls/reactive_fallback.cpp

@@ -34,9 +34,27 @@ NodeStatus ReactiveFallback::tick()
             if (current_child_node->status() != NodeStatus::RUNNING)
             {
                 // if not running already, tick it
+
+                if (parent_prt_ ==  nullptr)
+                {
+//                    std::cout << name() << " is root" << std::endl;
+
+                }
+                else
+                {
+
+//                    std::cout << name() << " is propagating halt to" << ((ControlNode*)parent_prt_)->name() << std::endl;
+
+
+                    ((ControlNode*)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();
 

src/controls/reactive_sequence.cpp

@@ -37,9 +37,27 @@ NodeStatus ReactiveSequence::tick()
             if (current_child_node->status() != NodeStatus::RUNNING)
             {
                 // if not running already, tick it
+                //before ticking it, make sure to halt the other running child.
+
+                if (parent_prt_ ==  nullptr)
+                {
+//                    std::cout << name() << " is root" << std::endl;
+
+                }
+                else
+                {
+
+//                    std::cout << name() << " is propagating halt to" << ((ControlNode*)parent_prt_)->name() << std::endl;
+
+
+                    ((ControlNode*)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);
             }

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(TreeNode *parent_ptr)
+{
+    parent_prt_ = parent_ptr;
+}
+
+TreeNode *TreeNode::parent_prt() const
+{
+    return parent_prt_;
+}
+
 }   // end namespace

tests/gtest_reactive_tree.cpp

@@ -64,6 +64,29 @@ struct ComplexReactiveTree : testing::Test
 TEST_F(ComplexReactiveTree, ConditionsFalse)
 {
 
+
+//    std::cout << fal_1.name() << " is the child of " << fal_1.parent_prt_->name() << std::endl;
+//    std::cout << fal_2.name() << " is the child of " << fal_2.parent_prt_->name() << std::endl;
+
+//    std::cout << action_1.name() << " is the child of " << action_1.parent_prt_->name() << std::endl;
+//    std::cout << action_2.name() << " is the child of " << action_2.parent_prt_->name() << std::endl;
+
+//    std::cout << condition_1.name() << " is the child of " << condition_1.parent_prt_->name() << std::endl;
+//    std::cout << condition_2.name() << " is the child of " << condition_2.parent_prt_->name() << std::endl;
+
+
+    if (root.parent_prt() ==  nullptr)
+    {
+        std::cout << root.name() << " is root" << std::endl;
+
+    }
+    else
+    {
+        std::cout << root.name() << " is the child of " << root.parent_prt()->name() << std::endl;
+    }
+
+
+
     auto t0 = std::chrono::high_resolution_clock::now();
 
 
@@ -82,6 +105,8 @@ TEST_F(ComplexReactiveTree, ConditionsFalse)
 
     std::this_thread::sleep_for(milliseconds(300));
 
+    std::cout << "condition 1 set to true" << std::endl;
+
     condition_1.setBoolean(true);
 
     state = root.executeTick();
@@ -98,16 +123,20 @@ TEST_F(ComplexReactiveTree, ConditionsFalse)
 
     std::this_thread::sleep_for(milliseconds(300));
     condition_1.setBoolean(false);
-    std::cout << "Ticking..." << std::endl;
+
+    std::cout << "condition 1 set to false" << std::endl;
+
+
+//    std::cout << "Ticking..." << std::endl;
     state = root.executeTick();
-    std::cout << "...done" << std::endl;
+//    std::cout << "...done" << std::endl;
 
 
 
     std::this_thread::sleep_for(milliseconds(300));
 
-    std::cout << action_1.startTimePoint().time_since_epoch().count() << std::endl;
-    std::cout << action_2.stopTimePoint().time_since_epoch().count() << std::endl;
+//    std::cout << action_1.startTimePoint().time_since_epoch().count() << std::endl;
+//    std::cout << action_2.stopTimePoint().time_since_epoch().count() << std::endl;
 
     ASSERT_TRUE(action_1.startTimePoint().time_since_epoch().count() >
                 action_2.stopTimePoint().time_since_epoch().count()  );

tests/include/action_test_node.h

@@ -4,6 +4,7 @@
 #include <ctime>
 #include <chrono>
 #include "behaviortree_cpp/action_node.h"
+#include <mutex>
 
 namespace BT
 {
@@ -60,29 +61,33 @@ class AsyncActionTest : public AsyncActionNode
 
     void setStartTimePoint(std::chrono::high_resolution_clock::time_point now)
     {
-        // TODO add lock guard
-       std::cout << this->name() << " Setting Start Time: " << now.time_since_epoch().count() << std::endl;
+        std::lock_guard<std::mutex> lock(start_time_mutex_);
+//       std::cout << this->name() << " Setting Start Time: " << now.time_since_epoch().count() << std::endl;
 
         start_time_ = now;
     }
 
-    std::chrono::high_resolution_clock::time_point startTimePoint() const
+    std::chrono::high_resolution_clock::time_point startTimePoint()
     {
+        std::lock_guard<std::mutex> lock(start_time_mutex_);
+
         return start_time_;
     }
 
 
     void setStopTimePoint(std::chrono::high_resolution_clock::time_point now)
     {
-        // TODO add lock guard
+        std::lock_guard<std::mutex> lock(stop_time_mutex_);
 
-        std::cout << this->name() << " Setting Stop Time: " << now.time_since_epoch().count()  << std::endl;
+//        std::cout << this->name() << " Setting Stop Time: " << now.time_since_epoch().count()  << std::endl;
 
         stop_time_ = now;
     }
 
-    std::chrono::high_resolution_clock::time_point stopTimePoint() const
+    std::chrono::high_resolution_clock::time_point stopTimePoint()
     {
+        std::lock_guard<std::mutex> lock(stop_time_mutex_);
+
         return stop_time_;
     }
 
@@ -94,6 +99,9 @@ class AsyncActionTest : public AsyncActionNode
     std::atomic<int> tick_count_;
     std::atomic_bool stop_loop_;
     std::chrono::high_resolution_clock::time_point start_time_, stop_time_;
+
+    std::mutex start_time_mutex_, stop_time_mutex_;
+
 };
 }
 

tests/src/action_test_node.cpp

@@ -36,7 +36,12 @@ BT::AsyncActionTest::~AsyncActionTest()
 
 BT::NodeStatus BT::AsyncActionTest::tick()
 {
-    setStartTimePoint(high_resolution_clock::now());
+
+    high_resolution_clock::time_point t2 = high_resolution_clock::now();
+
+//    std::cout << name() << " STARTED at"  << t2.time_since_epoch().count()  << std::endl;
+
+    setStartTimePoint(t2);
     setStatus(NodeStatus::RUNNING);
     using std::chrono::high_resolution_clock;
     tick_count_++;
@@ -46,12 +51,10 @@ BT::NodeStatus BT::AsyncActionTest::tick()
 
     while (!stop_loop_ && high_resolution_clock::now() < initial_time + time_)
     {
-//        high_resolution_clock::time_point t2 = high_resolution_clock::now();
-
-//        std::cout << this->name() << " Running. Time: " << std::chrono::system_clock::to_time_t(t2)  << std::endl;
 
 
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
     }
 
     setStopTimePoint(high_resolution_clock::now());
@@ -70,7 +73,19 @@ BT::NodeStatus BT::AsyncActionTest::tick()
 
 void BT::AsyncActionTest::halt()
 {
+    high_resolution_clock::time_point t2 = high_resolution_clock::now();
+
+//    std::cout << name() << " HALTED at "  << t2.time_since_epoch().count() << std::endl;
+
     stop_loop_ = true;
+ NodeStatus node_status;
+    do
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+       node_status = status();
+    } while (node_status == NodeStatus::RUNNING);
+
 }
 
 void BT::AsyncActionTest::setTime(BT::Duration time)