Skip to content

Commit

Permalink
fix issue #563
Browse files Browse the repository at this point in the history
  • Loading branch information
@facontidavide
facontidavide committed Jun 12, 2023
1 parent 73b7d0a commit 02e8dc2
Show file tree
Hide file tree
Showing 5 changed files with 133 additions and 158 deletions.
116 changes: 49 additions & 67 deletions include/behaviortree_cpp_v3/blackboard.h
View file
Expand Up @@ -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.
Expand All @@ -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.
Expand Down Expand Up @@ -116,65 +100,54 @@ 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
{
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<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);
Expand All @@ -197,6 +170,11 @@ class Blackboard
return entry_mutex_;
}

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

private:
struct Entry
{
Expand All @@ -211,11 +189,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
Expand Down
36 changes: 21 additions & 15 deletions include/behaviortree_cpp_v3/tree_node.h
View file
Expand Up @@ -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)
{
Expand Down
104 changes: 53 additions & 51 deletions src/blackboard.cpp
View file
Expand Up @@ -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)
Expand All @@ -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;
}
}

Expand All @@ -103,4 +64,45 @@ 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())
{
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

0 comments on commit 02e8dc2

Please sign in to comment.