Imported upstream version '3.8.6' of 'upstream'

Author: facontidavide

include/behaviortree_cpp_v3/blackboard.h

@@ -48,24 +48,7 @@ class Blackboard
      *
      * @return the pointer or nullptr if it fails.
      */
-  const Any* getAny(const std::string& key) const
-  {
-    std::unique_lock<std::mutex> lock(mutex_);
-    auto it = storage_.find(key);
-
-    if(it == storage_.end())
-    {
-      // Try with autoremapping. This should work recursively
-      if(autoremapping_)
-      {
-        if(auto parent = parent_bb_.lock()) {
-          return parent->getAny(key);
-        }
-      }
-      return nullptr;
-    }
-    return &(it->second->value);
-  }
+  const Any* getAny(const std::string& key) const;
 
   Any* getAny(const std::string& key)
   {
@@ -122,8 +105,15 @@ class Blackboard
     else
     {
       Any new_value(value);
+      std::shared_ptr<Blackboard::Entry> entry;
       lock.unlock();
-      entry = createEntryImpl(key, PortInfo(PortDirection::INOUT, new_value.type(), {}));
+      if(std::is_constructible<std::string, T>::value)
+      {
+        entry = createEntryImpl(key, PortInfo(PortDirection::INOUT));
+      }
+      else {
+        entry = createEntryImpl(key, PortInfo(PortDirection::INOUT, new_value.type(), {}));
+      }
       entry->value = new_value;
       return;
     }

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>behaviortree_cpp_v3</name>
-  <version>3.8.5</version>
+  <version>3.8.6</version>
   <description>
   This package provides the Behavior Trees core library.
   </description>

src/blackboard.cpp

@@ -7,6 +7,35 @@ void Blackboard::enableAutoRemapping(bool remapping)
   autoremapping_ = remapping;
 }
 
+const Any *Blackboard::getAny(const std::string &key) const
+{
+  std::unique_lock<std::mutex> lock(mutex_);
+  auto it = storage_.find(key);
+
+  if(it == storage_.end())
+  {
+    // Try with autoremapping. This should work recursively
+    auto remapping_it = internal_to_external_.find(key);
+    if (remapping_it != internal_to_external_.end())
+    {
+      const auto& remapped_key = remapping_it->second;
+      if (auto parent = parent_bb_.lock())
+      {
+        return parent->getAny(remapped_key);
+      }
+    }
+
+    else if(autoremapping_)
+    {
+      if(auto parent = parent_bb_.lock()) {
+        return parent->getAny(key);
+      }
+    }
+    return nullptr;
+  }
+  return &(it->second->value);
+}
+
 const PortInfo* Blackboard::portInfo(const std::string& key)
 {
   std::unique_lock<std::mutex> lock(mutex_);
@@ -75,12 +104,14 @@ Blackboard::createEntryImpl(const std::string &key, const PortInfo& info)
   auto storage_it = storage_.find(key);
   if(storage_it != storage_.end())
   {
-    const auto old_type = storage_it->second->port_info.type();
-    if (old_type && info.type() && old_type != info.type())
+    const auto& prev_info = storage_it->second->port_info;
+    if (prev_info.type() != info.type() &&
+        prev_info.isStronglyTyped() &&
+        info.isStronglyTyped())
     {
       throw LogicError("Blackboard: once declared, the type of a port "
                        "shall not change. Previously declared type [",
-                       BT::demangle(old_type), "] != new type [",
+                       BT::demangle(prev_info.type()), "] != new type [",
                        BT::demangle(info.type()), "]");
     }
     return storage_it->second;

src/controls/reactive_fallback.cpp

@@ -26,7 +26,10 @@ NodeStatus ReactiveFallback::tick()
     switch (child_status)
     {
       case NodeStatus::RUNNING: {
-        for (size_t i = index + 1; i < childrenCount(); i++)
+
+        // reset the previous children, to make sure that they are in IDLE state
+        // the next time we tick them
+        for (size_t i = 0; i < index; i++)
         {
           haltChild(i);
         }

src/controls/reactive_sequence.cpp

@@ -17,7 +17,6 @@ namespace BT
 NodeStatus ReactiveSequence::tick()
 {
   size_t success_count = 0;
-  size_t running_count = 0;
 
   for (size_t index = 0; index < childrenCount(); index++)
   {
@@ -27,9 +26,9 @@ NodeStatus ReactiveSequence::tick()
     switch (child_status)
     {
       case NodeStatus::RUNNING: {
-        running_count++;
-
-        for (size_t i = index + 1; i < childrenCount(); i++)
+        // reset the previous children, to make sure that they are in IDLE state
+        // the next time we tick them
+        for (size_t i = 0; i < index; i++)
         {
           haltChild(i);
         }
@@ -51,6 +50,7 @@ NodeStatus ReactiveSequence::tick()
     }   // end switch
   }     //end for
 
+
   if (success_count == childrenCount())
   {
     resetChildren();

tests/CMakeLists.txt

@@ -15,6 +15,7 @@ set(BT_TESTS
   gtest_blackboard.cpp
   gtest_blackboard_precondition.cpp
   gtest_ports.cpp
+  gtest_reactive.cpp
   navigation_test.cpp
   gtest_subtree.cpp
   gtest_switch.cpp

tests/gtest_fallback.cpp

@@ -149,8 +149,8 @@ TEST_F(ReactiveFallbackTest, Condition1ToTrue)
   BT::NodeStatus state = root.executeTick();
 
   ASSERT_EQ(NodeStatus::RUNNING, state);
-  ASSERT_EQ(NodeStatus::FAILURE, condition_1.status());
-  ASSERT_EQ(NodeStatus::FAILURE, condition_2.status());
+  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
   ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
 
   condition_1.setExpectedResult(NodeStatus::SUCCESS);
@@ -171,8 +171,8 @@ TEST_F(ReactiveFallbackTest, Condition2ToTrue)
   BT::NodeStatus state = root.executeTick();
 
   ASSERT_EQ(NodeStatus::RUNNING, state);
-  ASSERT_EQ(NodeStatus::FAILURE, condition_1.status());
-  ASSERT_EQ(NodeStatus::FAILURE, condition_2.status());
+  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
   ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
 
   condition_2.setExpectedResult(NodeStatus::SUCCESS);

tests/gtest_reactive.cpp

@@ -0,0 +1,69 @@
+#include <gtest/gtest.h>
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "test_helper.hpp"
+#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
+
+using BT::NodeStatus;
+using std::chrono::milliseconds;
+
+class SleepNode : public BT::StatefulActionNode
+{
+public:
+
+  SleepNode(const std::string& name, const BT::NodeConfiguration& config):
+    StatefulActionNode(name, config) {}
+
+  NodeStatus onStart() override {
+    count_ = 0;
+    return NodeStatus::RUNNING;
+  }
+
+  NodeStatus onRunning() override {
+    return ++count_ < 10 ? NodeStatus::RUNNING : NodeStatus::SUCCESS;
+  }
+
+  void onHalted() override {}
+
+  static BT::PortsList providedPorts(){
+    return {};
+  }
+
+private:
+  int count_ = 0;
+};
+
+
+TEST(Reactive, TestLogging)
+{
+  using namespace BT;
+
+  static const char* reactive_xml_text = R"(
+<root>
+  <BehaviorTree ID="Main">
+    <ReactiveSequence>
+      <TestA name="testA"/>
+      <AlwaysSuccess name="success"/>
+      <Sleep/>
+    </ReactiveSequence>
+  </BehaviorTree>
+</root>
+)";
+
+  BehaviorTreeFactory factory;
+
+  factory.registerNodeType<SleepNode>("Sleep");
+
+  std::array<int, 1> counters;
+  RegisterTestTick(factory, "Test", counters);
+
+  auto tree = factory.createTreeFromText(reactive_xml_text);
+  StdCoutLogger logger(tree);
+
+  auto ret = tree.tickRootWhileRunning();
+  ASSERT_EQ(ret, NodeStatus::SUCCESS);
+
+  int num_ticks = counters[0];
+  ASSERT_GE(num_ticks, 10);
+}
+
+

tests/gtest_sequence.cpp

@@ -226,7 +226,7 @@ TEST_F(ComplexSequenceTest, ComplexSequenceConditionsTrue)
   BT::NodeStatus state = root.executeTick();
 
   ASSERT_EQ(NodeStatus::RUNNING, state);
-  ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
+  ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status());
   ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
   ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
   ASSERT_EQ(NodeStatus::RUNNING, action_1.status());