blackboard refactoring to fix buggy _autoremap

Author: facontidavide

include/behaviortree_cpp/blackboard.h

@@ -61,24 +61,9 @@ class Blackboard
 
   virtual ~Blackboard() = default;
 
-  [[nodiscard]] const Entry* getEntry(const std::string& key) const
-  {
-    std::unique_lock<std::mutex> lock(mutex_);
-    // search first if this port was remapped
-    if(!internal_to_external_.empty())
-    {
-      if (auto parent = parent_bb_.lock())
-      {
-        auto remapping_it = internal_to_external_.find(key);
-        if (remapping_it != internal_to_external_.end())
-        {
-          return parent->getEntry(remapping_it->second);
-        }
-      }
-    }
-    auto it = storage_.find(key);
-    return (it == storage_.end()) ? nullptr : it->second.get();
-  }
+  void enableAutoRemapping(bool remapping);
+
+  [[nodiscard]] const Entry* getEntry(const std::string& key) const;
 
   [[nodiscard]] Entry* getEntry(const std::string& key)
   {
@@ -158,30 +143,18 @@ class Blackboard
   {
     std::unique_lock lock(mutex_);
 
-    // search first if this port was remapped.
-    // Change the parent_bb_ in that case
-    if(!internal_to_external_.empty())
-    {
-      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())
-        {
-          parent->set(remapped_key, value);
-          return;
-        }
-      }
-    }
-
     // check local storage
     auto it = storage_.find(key);
     if (it == storage_.end())
     {
       // create a new entry
       Any new_value(value);
       PortInfo new_port(PortDirection::INOUT, new_value.type(), {});
-      storage_.emplace(key, std::make_unique<Entry>(std::move(new_value), new_port));
+      lock.unlock();
+      auto entry = createEntryImpl(key, new_port);
+      lock.lock();
+      storage_.insert( {key, entry} );
+      entry->value = new_value;
     }
     else
     {
@@ -236,15 +209,13 @@ class Blackboard
     }
   }
 
-  void setPortInfo(const std::string &key, const PortInfo& info);
-
-  [[nodiscard]] const PortInfo* portInfo(const std::string& key);
+   [[nodiscard]] const PortInfo* portInfo(const std::string& key);
 
   void addSubtreeRemapping(StringView internal, StringView external);
 
   void debugMessage() const;
 
-  [[nodiscard]] std::vector<StringView> getKeys(bool include_remapped = true) const;
+  [[nodiscard]] std::vector<StringView> getKeys() const;
 
   void clear()
   {
@@ -258,13 +229,21 @@ class Blackboard
     return entry_mutex_;
   }
 
+  void createEntry(const std::string& key, const PortInfo& info)
+  {
+    createEntryImpl(key, info);
+  }
+
 private:
   mutable std::mutex mutex_;
   mutable std::recursive_mutex entry_mutex_;
-  std::unordered_map<std::string, std::unique_ptr<Entry>> storage_;
+  std::unordered_map<std::string, std::shared_ptr<Entry>> storage_;
   std::weak_ptr<Blackboard> parent_bb_;
   std::unordered_map<std::string, std::string> internal_to_external_;
 
+  std::shared_ptr<Entry> createEntryImpl(const std::string &key, const PortInfo& info);
+
+  bool autoremapping_ = false;
 };
 
 }   // namespace BT

include/behaviortree_cpp/scripting/operators.hpp

@@ -365,12 +365,17 @@ struct ExprAssignment : ExprBase
             // variable doesn't exist, create it if using operator assign_create
             if(op == assign_create)
             {
-                env.vars->setPortInfo(key, PortInfo());
+                env.vars->createEntry(key, PortInfo());
                 entry = env.vars->getEntry(key);
             }
             else {
                 // fail otherwise
-                throw std::runtime_error("Can't create a new variable");
+                char buffer[1014];
+                sprintf(buffer,
+                        "The blackboard entry [%s] doesn't exist, yet.\n"
+                        "If you want to create a new one, use the operator "
+                        "[:=] instead of [=]", key.c_str());
+                throw std::runtime_error(buffer);
             }
         }
         auto value = rhs->evaluate(env);

src/blackboard.cpp

@@ -2,57 +2,25 @@
 
 namespace BT
 {
-void Blackboard::setPortInfo(const std::string& key, const PortInfo& info)
-{
-  std::unique_lock<std::mutex> lock(mutex_);
 
-  if (auto parent = parent_bb_.lock())
-  {
-    auto remapping_it = internal_to_external_.find(key);
-    if (remapping_it != internal_to_external_.end())
-    {
-      parent->setPortInfo(remapping_it->second, info);
-      return;
-    }
-  }
+void Blackboard::enableAutoRemapping(bool remapping)
+{
+  autoremapping_ = remapping;
+}
 
+const Blackboard::Entry *Blackboard::getEntry(const std::string &key) const
+{
+  std::unique_lock<std::mutex> lock(mutex_);
   auto it = storage_.find(key);
-  if (it == storage_.end())
-  {
-    storage_.emplace(key, std::make_unique<Entry>(info));
-  }
-  else
-  {
-    auto old_type = it->second->port_info.type();
-    if (old_type != info.type())
-    {
-      throw LogicError("Blackboard::set() failed: once declared, the type of a port "
-                       "shall not change. Declared type [",
-                       BT::demangle(old_type), "] != current type [",
-                       BT::demangle(info.type()), "]");
-    }
-  }
+  return (it == storage_.end()) ? nullptr : it->second.get();
 }
 
 const PortInfo* Blackboard::portInfo(const std::string& key)
 {
   std::unique_lock<std::mutex> lock(mutex_);
 
-  if (auto parent = parent_bb_.lock())
-  {
-    auto remapping_it = internal_to_external_.find(key);
-    if (remapping_it != internal_to_external_.end())
-    {
-      return parent->portInfo(remapping_it->second);
-    }
-  }
-
   auto it = storage_.find(key);
-  if (it == storage_.end())
-  {
-    return nullptr;
-  }
-  return &(it->second->port_info);
+  return (it == storage_.end()) ? nullptr : &(it->second->port_info);
 }
 
 void Blackboard::addSubtreeRemapping(StringView internal, StringView external)
@@ -82,27 +50,70 @@ void Blackboard::debugMessage() const
   }
 }
 
-std::vector<StringView> Blackboard::getKeys(bool include_remapped) const
+std::vector<StringView> Blackboard::getKeys() const
 {
-  const size_t N = storage_.size() + (include_remapped ? internal_to_external_.size() : 0 );
-  if (N == 0)
+  if (storage_.empty())
   {
     return {};
   }
   std::vector<StringView> out;
-  out.reserve(N);
+  out.reserve(storage_.size());
   for (const auto& entry_it : storage_)
   {
     out.push_back(entry_it.first);
   }
-  if(include_remapped)
+  return out;
+}
+
+std::shared_ptr<Blackboard::Entry>
+Blackboard::createEntryImpl(const std::string& key, const PortInfo& info)
+{
+  std::unique_lock<std::mutex> lock(mutex_);
+  // This function might be called recursively, when we do remapping, because we move
+  // to the top scope to find already existing  entries
+
+  // search if exists already
+  auto storage_it = storage_.find(key);
+  if(storage_it != storage_.end())
   {
-    for (const auto& [key, remapped] : internal_to_external_)
+    const auto old_type = storage_it->second->port_info.type();
+    if (old_type != info.type() &&
+        old_type != typeid(BT::PortInfo::AnyTypeAllowed) &&
+        info.type() != typeid(BT::PortInfo::AnyTypeAllowed))
     {
-      out.push_back(key);
+      throw LogicError("Blackboard: once declared, the type of a port "
+                       "shall not change. Previously declared type [",
+                       BT::demangle(old_type), "] != new type [",
+                       BT::demangle(info.type()), "]");
     }
+    return storage_it->second;
   }
-  return out;
+
+  std::shared_ptr<Entry> entry;
+
+  // manual remapping first
+  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())
+    {
+      entry = parent->createEntryImpl(remapped_key, info);
+    }
+  }
+  else if(autoremapping_)
+  {
+    if (auto parent = parent_bb_.lock())
+    {
+      entry = parent->createEntryImpl(key, info);
+    }
+  }
+  else // not remapped, nor found. Create locally.
+  {
+    entry = std::make_shared<Entry>(info);
+  }
+  storage_.insert( {key, entry} );
+  return entry;
 }
 
 }   // namespace BT

src/xml_parsing.cpp

@@ -646,7 +646,7 @@ TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement* element,
         else
         {
           // not found, insert for the first time.
-          blackboard->setPortInfo(port_key, port_info);
+          blackboard->createEntry(port_key, port_info);
         }
       }
     }
@@ -743,16 +743,15 @@ void BT::XMLParser::Pimpl::recursivelyCreateSubtree(
 
       std::set<StringView> mapped_keys;
 
-      bool do_autoremap = false;
-
       for (auto attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next())
       {
         const char* attr_name = attr->Name();
         const char* attr_value = attr->Value();
 
         if (StrEqual(attr_name, "_autoremap"))
         {
-          do_autoremap = convertFromString<bool>(attr_value);
+          const bool do_autoremap = convertFromString<bool>(attr_value);
+          new_bb->enableAutoRemapping(do_autoremap);
           continue;
         }
         if (!IsAllowedPortName(attr->Name()))
@@ -775,20 +774,6 @@ void BT::XMLParser::Pimpl::recursivelyCreateSubtree(
         }
       }
 
-      if (do_autoremap)
-      {
-        std::vector<std::string> remapped_ports;
-        auto new_root_element = tree_roots[node->name()]->FirstChildElement();
-
-        getPortsRecursively(new_root_element, remapped_ports);
-        for (const auto& port : remapped_ports)
-        {
-          if (mapped_keys.count(port) == 0)
-          {
-            new_bb->addSubtreeRemapping(port, port);
-          }
-        }
-      }
       std::string subtree_ID = element->Attribute("ID");
       std::string subtree_name = subtree->instance_name;
       if(!subtree_name.empty()) {

tests/gtest_factory.cpp

@@ -189,7 +189,7 @@ static const char* xml_ports_subtree = R"(
     <Sequence>
       <SaySomething message="{hello_msg}" />
       <SaySomething message="{bye_msg}" />
-      <Script code=" output='done!' " />
+      <Script code=" output:='done!' " />
     </Sequence>
   </BehaviorTree>
 

tests/gtest_subtree.cpp

@@ -330,22 +330,22 @@ TEST(SubTree, StringConversions_Issue530)
 TEST(SubTree, SubtreeIssue563)
 {
   static const char* xml_text = R"(
-<root main_tree_to_execute="Tree1">
+<root BTCPP_format="4" >
 
 <BehaviorTree ID="Tree1">
   <Sequence>
     <SetBlackboard output_key="the_message" value="hello world"/>
-    <SubTreePlus ID="Tree2" __autoremap="true"/>
+    <SubTree ID="Tree2" _autoremap="true"/>
     <SaySomething message="{reply}" />
   </Sequence>
 </BehaviorTree>
 
 <BehaviorTree ID="Tree2">
-    <SubTreePlus ID="Tree3" __autoremap="true"/>
+    <SubTree ID="Tree3" _autoremap="true"/>
 </BehaviorTree>
 
 <BehaviorTree ID="Tree3">
-    <SubTreePlus ID="Talker" __autoremap="true"/>
+    <SubTree ID="Talker" _autoremap="true"/>
 </BehaviorTree>
 
 <BehaviorTree ID="Talker">
@@ -359,8 +359,9 @@ TEST(SubTree, SubtreeIssue563)
 
   BehaviorTreeFactory factory;
   factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+  factory.registerBehaviorTreeFromText(xml_text);
+  Tree tree = factory.createTree("Tree1");
 
-  Tree tree = factory.createTreeFromText(xml_text);
   auto ret = tree.tickOnce();
   ASSERT_EQ(ret, NodeStatus::SUCCESS);