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

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
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
Loading

0 comments on commit 79e25b8

Please sign in to comment.