diff --git a/include/behaviortree_cpp_v3/blackboard.h b/include/behaviortree_cpp_v3/blackboard.h index 29b18542c..53ffb4595 100644 --- a/include/behaviortree_cpp_v3/blackboard.h +++ b/include/behaviortree_cpp_v3/blackboard.h @@ -40,6 +40,8 @@ class Blackboard virtual ~Blackboard() = default; + void enableAutoRemapping(bool remapping); + /** * @brief The method getAny allow the user to access directly the type * erased value. @@ -49,33 +51,15 @@ class Blackboard const Any* getAny(const std::string& key) const { std::unique_lock lock(mutex_); - // search first if this port was remapped - if (auto parent = parent_bb_.lock()) - { - auto remapping_it = internal_to_external_.find(key); - if (remapping_it != internal_to_external_.end()) - { - return parent->getAny(remapping_it->second); - } - } auto it = storage_.find(key); - return (it == storage_.end()) ? nullptr : &(it->second.value); + return (it != storage_.end()) ? &(it->second->value) : nullptr; } Any* getAny(const std::string& key) { - std::unique_lock lock(mutex_); - // search first if this port was remapped - if (auto parent = parent_bb_.lock()) - { - auto remapping_it = internal_to_external_.find(key); - if (remapping_it != internal_to_external_.end()) - { - return parent->getAny(remapping_it->second); - } - } - auto it = storage_.find(key); - return (it == storage_.end()) ? nullptr : &(it->second.value); + // "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(static_cast(*this).getAny(key)); } /** Return true if the entry with the given key was found. @@ -116,65 +100,54 @@ class Blackboard std::unique_lock lock_entry(entry_mutex_); std::unique_lock lock(mutex_); - // search first if this port was remapped. - // Change the parent_bb_ in that case - 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); + std::shared_ptr entry; if (it != storage_.end()) { - const PortInfo& port_info = it->second.port_info; - auto& previous_any = it->second.value; - const auto previous_type = port_info.type(); + entry = it->second; + } + else + { + lock.unlock(); + entry = createEntryImpl(key, PortInfo()); + entry->value = Any(value); + return; + } - Any new_value(value); + const PortInfo& port_info = entry->port_info; + auto& previous_any = entry->value; + const auto previous_type = port_info.type(); - if (previous_type && *previous_type != typeid(T) && - *previous_type != new_value.type()) + Any new_value(value); + + if (previous_type && *previous_type != typeid(T) && + *previous_type != new_value.type()) + { + bool mismatching = true; + if (std::is_constructible::value) { - bool mismatching = true; - if (std::is_constructible::value) + Any any_from_string = port_info.parseString(value); + if (any_from_string.empty() == false) { - Any any_from_string = port_info.parseString(value); - if (any_from_string.empty() == false) - { - mismatching = false; - new_value = std::move(any_from_string); - } + mismatching = false; + new_value = std::move(any_from_string); } + } - if (mismatching) - { - debugMessage(); + if (mismatching) + { + debugMessage(); - throw LogicError("Blackboard::set() failed: once declared, the type of a port " - "shall not change. " - "Declared type [", - BT::demangle(previous_type), "] != current type [", - BT::demangle(typeid(T)), "]"); - } + throw LogicError("Blackboard::set() failed: once declared, the type of a port " + "shall not change. Declared type [", + BT::demangle(previous_type), "] != current type [", + BT::demangle(typeid(T)), "]"); } - previous_any = std::move(new_value); } - else - { // create for the first time without any info - storage_.emplace(key, Entry(Any(value), PortInfo())); - } - return; + previous_any = std::move(new_value); } - void setPortInfo(std::string key, const PortInfo& info); - const PortInfo* portInfo(const std::string& key); void addSubtreeRemapping(StringView internal, StringView external); @@ -197,6 +170,11 @@ class Blackboard return entry_mutex_; } + void createEntry(const std::string& key, const PortInfo& info) + { + createEntryImpl(key, info); + } + private: struct Entry { @@ -211,11 +189,15 @@ class Blackboard {} }; + std::shared_ptr createEntryImpl(const std::string& key, const PortInfo& info); + mutable std::mutex mutex_; mutable std::mutex entry_mutex_; - std::unordered_map storage_; + std::unordered_map> storage_; std::weak_ptr parent_bb_; std::unordered_map internal_to_external_; + + bool autoremapping_ = false; }; } // namespace BT diff --git a/include/behaviortree_cpp_v3/tree_node.h b/include/behaviortree_cpp_v3/tree_node.h index d626d8619..d6d203125 100644 --- a/include/behaviortree_cpp_v3/tree_node.h +++ b/include/behaviortree_cpp_v3/tree_node.h @@ -258,24 +258,30 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const std::unique_lock entry_lock(config_.blackboard->entryMutex()); const Any* val = config_.blackboard->getAny(static_cast(remapped_key)); - if (val && val->empty() == false) + + if(!val) { - if (std::is_same::value == false && - val->type() == typeid(std::string)) - { - destination = convertFromString(val->cast()); - } - else - { - destination = val->cast(); - } - return {}; + return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to " + "find the port [", key, + "] remapped to BB [", remapped_key, "]")); + } + + if(val->empty()) + { + return nonstd::make_unexpected(StrCat("getInput() failed because the port [", key, + "] remapped to BB [", remapped_key, "] was found," + "but its content was not initialized correctly")); } - return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to " - "find the " - "key [", - key, "] remapped to [", remapped_key, "]")); + if (!std::is_same::value && val->type() == typeid(std::string)) + { + destination = convertFromString(val->cast()); + } + else + { + destination = val->cast(); + } + return {}; } catch (std::exception& err) { diff --git a/src/blackboard.cpp b/src/blackboard.cpp index ed64f6a7c..7ad14dee1 100644 --- a/src/blackboard.cpp +++ b/src/blackboard.cpp @@ -2,59 +2,17 @@ namespace BT { -void Blackboard::setPortInfo(std::string key, const PortInfo& info) +void Blackboard::enableAutoRemapping(bool remapping) { - std::unique_lock 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; - } - } - - auto it = storage_.find(key); - if (it == storage_.end()) - { - storage_.emplace(key, Entry(info)); - } - else - { - auto old_type = it->second.port_info.type(); - if (old_type && *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()), "]"); - } - } + autoremapping_ = remapping; } const PortInfo* Blackboard::portInfo(const std::string& key) { std::unique_lock 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) @@ -65,26 +23,29 @@ void Blackboard::addSubtreeRemapping(StringView internal, StringView external) void Blackboard::debugMessage() const { - for (const auto& entry_it : storage_) + for (const auto& it: storage_) { - auto port_type = entry_it.second.port_info.type(); + const auto& key = it.first; + const auto& entry = it.second; + + auto port_type = entry->port_info.type(); if (!port_type) { - port_type = &(entry_it.second.value.type()); + port_type = &(entry->value.type()); } - std::cout << entry_it.first << " (" << demangle(port_type) << ") -> "; + std::cout << key << " (" << demangle(port_type) << ") -> "; if (auto parent = parent_bb_.lock()) { - auto remapping_it = internal_to_external_.find(entry_it.first); + auto remapping_it = internal_to_external_.find(key); if (remapping_it != internal_to_external_.end()) { std::cout << "remapped to parent [" << remapping_it->second << "]" << std::endl; continue; } } - std::cout << ((entry_it.second.value.empty()) ? "empty" : "full") << std::endl; + std::cout << ((entry->value.empty()) ? "empty" : "full") << std::endl; } } @@ -103,4 +64,45 @@ std::vector Blackboard::getKeys() const return out; } +std::shared_ptr +Blackboard::createEntryImpl(const std::string &key, const PortInfo& info) +{ + std::unique_lock 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()) + { + return storage_it->second; + } + + std::shared_ptr 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(info); + } + storage_.insert( {key, entry} ); + return entry; +} + } // namespace BT diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index f30bd4748..5ce65ae92 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -574,7 +574,7 @@ TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement* element, if (!prev_info) { // not found, insert for the first time. - blackboard->setPortInfo(port_key, port_info); + blackboard->createEntry(port_key, port_info); } else { @@ -708,8 +708,6 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, output_tree.blackboard_stack.emplace_back(new_bb); std::set mapped_keys; - bool do_autoremap = false; - for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) { @@ -722,7 +720,8 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, } if (StrEqual(attr_name, "__autoremap")) { - do_autoremap = convertFromString(attr_value); + bool do_autoremap = convertFromString(attr_value); + new_bb->enableAutoRemapping(do_autoremap); continue; } @@ -741,21 +740,6 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, } } - if (do_autoremap) - { - std::vector 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); - } - } - } - recursivelyCreateTree(node->name(), output_tree, new_bb, node); } } diff --git a/tests/gtest_subtree.cpp b/tests/gtest_subtree.cpp index c99d5257f..11b827a29 100644 --- a/tests/gtest_subtree.cpp +++ b/tests/gtest_subtree.cpp @@ -20,7 +20,7 @@ TEST(SubTree, SiblingPorts_Issue_72) - + )"; @@ -146,16 +146,13 @@ TEST(SubTree, SubtreePlusA) - - - - + )"; @@ -318,8 +315,9 @@ TEST(SubTree, SubtreeIssue563) - + + @@ -332,7 +330,10 @@ TEST(SubTree, SubtreeIssue563) + + + )";