Skip to content

Commit

Permalink
Merge pull request #583 from BehaviorTree/issue563
Browse files Browse the repository at this point in the history
Issue563
facontidavide authored Jun 13, 2023
2 parents da88d88 + 90d7bf9 commit 79e25b8
Showing 5 changed files with 176 additions and 157 deletions.
117 changes: 50 additions & 67 deletions include/behaviortree_cpp_v3/blackboard.h
Original file line number Diff line number Diff line change
@@ -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<std::mutex> 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<std::mutex> 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<Any*>(static_cast<const Blackboard&>(*this).getAny(key));
}

/** Return true if the entry with the given key was found.
@@ -116,65 +100,55 @@ class Blackboard
std::unique_lock<std::mutex> lock_entry(entry_mutex_);
std::unique_lock<std::mutex> 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> 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
{
Any new_value(value);
lock.unlock();
entry = createEntryImpl(key, PortInfo(PortDirection::INOUT, new_value.type(), {}));
entry->value = new_value;
return;
}

const PortInfo& port_info = entry->port_info;
auto& previous_any = entry->value;
const auto previous_type = port_info.type();

Any new_value(value);

if (previous_type && *previous_type != typeid(T) &&
*previous_type != new_value.type())
if (previous_type && *previous_type != typeid(T) &&
*previous_type != new_value.type())
{
bool mismatching = true;
if (std::is_constructible<StringView, T>::value)
{
bool mismatching = true;
if (std::is_constructible<StringView, T>::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 +171,11 @@ class Blackboard
return entry_mutex_;
}

void createEntry(const std::string& key, const PortInfo& info)
{
createEntryImpl(key, info);
}

private:
struct Entry
{
@@ -211,11 +190,15 @@ class Blackboard
{}
};

std::shared_ptr<Entry> createEntryImpl(const std::string& key, const PortInfo& info);

mutable std::mutex mutex_;
mutable std::mutex entry_mutex_;
std::unordered_map<std::string, 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_;

bool autoremapping_ = false;
};

} // namespace BT
36 changes: 21 additions & 15 deletions include/behaviortree_cpp_v3/tree_node.h
Original file line number Diff line number Diff line change
@@ -258,24 +258,30 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const

std::unique_lock<std::mutex> entry_lock(config_.blackboard->entryMutex());
const Any* val = config_.blackboard->getAny(static_cast<std::string>(remapped_key));
if (val && val->empty() == false)

if(!val)
{
if (std::is_same<T, std::string>::value == false &&
val->type() == typeid(std::string))
{
destination = convertFromString<T>(val->cast<std::string>());
}
else
{
destination = val->cast<T>();
}
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<T, std::string>::value && val->type() == typeid(std::string))
{
destination = convertFromString<T>(val->cast<std::string>());
}
else
{
destination = val->cast<T>();
}
return {};
}
catch (std::exception& err)
{
112 changes: 61 additions & 51 deletions src/blackboard.cpp
Original file line number Diff line number Diff line change
@@ -2,59 +2,17 @@

namespace BT
{
void Blackboard::setPortInfo(std::string key, const PortInfo& info)
void Blackboard::enableAutoRemapping(bool remapping)
{
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;
}
}

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<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)
@@ -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,53 @@ std::vector<StringView> Blackboard::getKeys() const
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())
{
const auto old_type = storage_it->second->port_info.type();
if (old_type && info.type() && old_type != info.type())
{
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;
}

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
22 changes: 3 additions & 19 deletions src/xml_parsing.cpp
Original file line number Diff line number Diff line change
@@ -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<StringView> 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<bool>(attr_value);
bool do_autoremap = convertFromString<bool>(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<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);
}
}
}

recursivelyCreateTree(node->name(), output_tree, new_bb, node);
}
}
46 changes: 41 additions & 5 deletions tests/gtest_subtree.cpp
Original file line number Diff line number Diff line change
@@ -20,7 +20,7 @@ TEST(SubTree, SiblingPorts_Issue_72)
</BehaviorTree>
<BehaviorTree ID="mySubtree">
<SaySomething ID="AlwaysSuccess" message="{param}" />
<SaySomething message="{param}" />
</BehaviorTree>
</root> )";

@@ -146,16 +146,13 @@ TEST(SubTree, SubtreePlusA)
<BehaviorTree ID="MainTree">
<Sequence>
<SetBlackboard value="Hello" output_key="myParam" />
<SubTreePlus ID="mySubtree" param="{myParam}" />
<SubTreePlus ID="mySubtree" param="World" />
<SetBlackboard value="Auto remapped" output_key="param" />
<SubTreePlus ID="mySubtree" __autoremap="1" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="mySubtree">
<SaySomething message="{param}" />
<SaySomething message="{param}" />
</BehaviorTree>
</root> )";

@@ -310,3 +307,42 @@ TEST(SubTree, SubtreeIssue433)

ASSERT_EQ(ret, BT::NodeStatus::SUCCESS);
}

TEST(SubTree, SubtreeIssue563)
{
static const char* xml_text = R"(
<root main_tree_to_execute="Tree1">
<BehaviorTree ID="Tree1">
<Sequence>
<SetBlackboard output_key="the_message" value="hello world"/>
<SubTreePlus ID="Tree2" __autoremap="true"/>
<SaySomething message="{reply}" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="Tree2">
<SubTreePlus ID="Tree3" __autoremap="true"/>
</BehaviorTree>
<BehaviorTree ID="Tree3">
<SubTreePlus ID="Talker" __autoremap="true"/>
</BehaviorTree>
<BehaviorTree ID="Talker">
<Sequence>
<SaySomething message="{the_message}" />
<SetBlackboard output_key="reply" value="done"/>
</Sequence>
</BehaviorTree>
</root>)";

BehaviorTreeFactory factory;
factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");

Tree tree = factory.createTreeFromText(xml_text);
auto ret = tree.tickRoot();
ASSERT_EQ(ret, NodeStatus::SUCCESS);

}

0 comments on commit 79e25b8

Please sign in to comment.