fis issue #592

Author: facontidavide

include/behaviortree_cpp/blackboard.h

@@ -63,9 +63,9 @@ class Blackboard
 
   void enableAutoRemapping(bool remapping);
 
-  [[nodiscard]] const Entry* getEntry(const std::string& key) const;
+  [[nodiscard]] const std::shared_ptr<Entry> getEntry(const std::string& key) const;
 
-  [[nodiscard]] Entry* getEntry(const std::string& key);
+  [[nodiscard]] std::shared_ptr<Blackboard::Entry> getEntry(const std::string& key);
 
   [[nodiscard]] AnyPtrLocked getAnyLocked(const std::string& key);
 

include/behaviortree_cpp/scripting/operators.hpp

@@ -359,7 +359,7 @@ struct ExprAssignment : ExprBase
         }
         const auto& key = varname->name;
 
-        Blackboard::Entry* entry = env.vars->getEntry(key);
+        auto entry = env.vars->getEntry(key);
         if( !entry )
         {
             // variable doesn't exist, create it if using operator assign_create

src/blackboard.cpp

@@ -8,13 +8,6 @@ void Blackboard::enableAutoRemapping(bool remapping)
   autoremapping_ = remapping;
 }
 
-Blackboard::Entry *Blackboard::getEntry(const std::string &key)
-{
-  // "Avoid Duplication in const and Non-const Member Function,"
-  // on p. 23, in Item 3 "Use const whenever possible," in Effective C++, 3d ed
-  return const_cast<Entry*>( static_cast<const Blackboard &>(*this).getEntry(key));
-}
-
 AnyPtrLocked Blackboard::getAnyLocked(const std::string &key)
 {
   if(auto entry = getEntry(key))
@@ -43,13 +36,67 @@ Any *Blackboard::getAny(const std::string &key)
   return const_cast<Any*>(getAnyLocked(key).get());
 }
 
-const Blackboard::Entry *Blackboard::getEntry(const std::string &key) const
+const std::shared_ptr<Blackboard::Entry> Blackboard::getEntry(const std::string &key) const
 {
   std::unique_lock<std::mutex> lock(mutex_);
   auto it = storage_.find(key);
-  return (it == storage_.end()) ? nullptr : it->second.get();
+  if(it != storage_.end()) {
+    return it->second;
+  }
+  // not found. Try autoremapping
+  if (auto parent = parent_bb_.lock())
+  {
+    auto remap_it = internal_to_external_.find(key);
+    if (remap_it != internal_to_external_.cend())
+    {
+      auto const& new_key = remap_it->second;
+      return parent->getEntry(new_key);
+    }
+    if(autoremapping_)
+    {
+      return parent->getEntry(key);
+    }
+  }
+  return {};
 }
 
+
+std::shared_ptr<Blackboard::Entry> Blackboard::getEntry(const std::string &key)
+{
+  std::unique_lock<std::mutex> lock(mutex_);
+  auto it = storage_.find(key);
+  if(it != storage_.end()) {
+    return it->second;
+  }
+
+  // not found. Try autoremapping
+  if (auto parent = parent_bb_.lock())
+  {
+    auto remap_it = internal_to_external_.find(key);
+    if (remap_it != internal_to_external_.cend())
+    {
+      auto const& new_key = remap_it->second;
+      auto entry = parent->getEntry(new_key);
+      if(entry)
+      {
+        storage_.insert({key, entry});
+      }
+      return entry;
+    }
+    if(autoremapping_)
+    {
+      auto entry = parent->getEntry(key);
+      if(entry)
+      {
+        storage_.insert({key, entry});
+      }
+      return entry;
+    }
+  }
+  return {};
+}
+
+
 const PortInfo* Blackboard::portInfo(const std::string& key)
 {
   std::unique_lock<std::mutex> lock(mutex_);

tests/gtest_subtree.cpp

@@ -2,6 +2,7 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "../sample_nodes/dummy_nodes.h"
 #include "../sample_nodes/movebase_node.h"
+#include "test_helper.hpp"
 
 using namespace BT;
 
@@ -306,6 +307,7 @@ class ModifyPose : public BT::SyncActionNode
   }
 };
 
+
 TEST(SubTree, StringConversions_Issue530)
 {
   const char* xml_text = R"(
@@ -327,6 +329,26 @@ TEST(SubTree, StringConversions_Issue530)
   tree.tickOnce();
 }
 
+class NaughtyNav2Node : public BT::SyncActionNode
+{
+public:
+  NaughtyNav2Node(const std::string& name, const BT::NodeConfiguration& config) :
+    BT::SyncActionNode(name, config)
+  {
+    std::cout << "CTOR:" << config.blackboard->get<std::string>("ros_node") << std::endl;
+  }
+
+  BT::NodeStatus tick() override
+  {
+    std::cout << "tick:" << config().blackboard->get<std::string>("ros_node") << std::endl;
+    return BT::NodeStatus::SUCCESS;
+  }
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+};
+
 TEST(SubTree, SubtreeIssue563)
 {
   static const char* xml_text = R"(
@@ -352,17 +374,59 @@ TEST(SubTree, SubtreeIssue563)
   <Sequence>
     <SaySomething message="{the_message}" />
     <Script code=" reply:='done' "/>
+    <NaughtyNav2Node/>
   </Sequence>
 </BehaviorTree>
 
 </root>)";
 
   BehaviorTreeFactory factory;
   factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+  factory.registerNodeType<NaughtyNav2Node>("NaughtyNav2Node");
+
   factory.registerBehaviorTreeFromText(xml_text);
-  Tree tree = factory.createTree("Tree1");
+
+  auto blackboard = BT::Blackboard::create();
+  blackboard->set<std::string>("ros_node", "nav2_shouldnt_do_this");
+
+  Tree tree = factory.createTree("Tree1", blackboard);
 
   auto ret = tree.tickOnce();
   ASSERT_EQ(ret, NodeStatus::SUCCESS);
+}
+
 
+TEST(SubTree, SubtreeIssue592)
+{
+  static const char* xml_text = R"(
+<root BTCPP_format="4" >
+
+  <BehaviorTree ID="Outer_Tree">
+    <Sequence>
+      <Script code="variable := 'test'"/>
+      <SubTree ID="Inner_Tree"
+               variable="{variable}"
+               _autoremap="false"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="Inner_Tree">
+    <Sequence>
+      <TestA _skipIf="variable != 'test'"/>
+    </Sequence>
+  </BehaviorTree>
+
+</root>)";
+
+
+  BehaviorTreeFactory factory;
+  std::array<int, 1> counters;
+  RegisterTestTick(factory, "Test", counters);
+
+  factory.registerBehaviorTreeFromText(xml_text);
+  Tree tree = factory.createTree("Outer_Tree");
+
+  auto ret = tree.tickWhileRunning();
+  ASSERT_EQ(ret, NodeStatus::SUCCESS);
+  ASSERT_EQ(counters[0], 1);
 }