Skip to content

Commit 037faeb

Browse files
facontidavidefacontidavide
committed
fix issue #563 
1 parent 39309cf commit 037faeb

File tree

5 files changed

+133
-158
lines changed

5 files changed

+133
-158
lines changed

‎include/behaviortree_cpp_v3/blackboard.h

Lines changed: 49 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,8 @@ class Blackboard
4040

4141
virtual ~Blackboard() = default;
4242

43+
void enableAutoRemapping(bool remapping);
44+
4345
/**
4446
* @brief The method getAny allow the user to access directly the type
4547
* erased value.
@@ -49,33 +51,15 @@ class Blackboard
4951
const Any* getAny(const std::string& key) const
5052
{
5153
std::unique_lock<std::mutex> lock(mutex_);
52-
// search first if this port was remapped
53-
if (auto parent = parent_bb_.lock())
54-
{
55-
auto remapping_it = internal_to_external_.find(key);
56-
if (remapping_it != internal_to_external_.end())
57-
{
58-
return parent->getAny(remapping_it->second);
59-
}
60-
}
6154
auto it = storage_.find(key);
62-
return (it == storage_.end()) ? nullptr : &(it->second.value);
55+
return (it != storage_.end()) ? &(it->second->value) : nullptr;
6356
}
6457

6558
Any* getAny(const std::string& key)
6659
{
67-
std::unique_lock<std::mutex> lock(mutex_);
68-
// search first if this port was remapped
69-
if (auto parent = parent_bb_.lock())
70-
{
71-
auto remapping_it = internal_to_external_.find(key);
72-
if (remapping_it != internal_to_external_.end())
73-
{
74-
return parent->getAny(remapping_it->second);
75-
}
76-
}
77-
auto it = storage_.find(key);
78-
return (it == storage_.end()) ? nullptr : &(it->second.value);
60+
// "Avoid Duplication in const and Non-const Member Function,"
61+
// on p. 23, in Item 3 "Use const whenever possible," in Effective C++, 3d ed
62+
return const_cast<Any*>(static_cast<const Blackboard&>(*this).getAny(key));
7963
}
8064

8165
/** Return true if the entry with the given key was found.
@@ -116,65 +100,54 @@ class Blackboard
116100
std::unique_lock<std::mutex> lock_entry(entry_mutex_);
117101
std::unique_lock<std::mutex> lock(mutex_);
118102

119-
// search first if this port was remapped.
120-
// Change the parent_bb_ in that case
121-
auto remapping_it = internal_to_external_.find(key);
122-
if (remapping_it != internal_to_external_.end())
123-
{
124-
const auto& remapped_key = remapping_it->second;
125-
if (auto parent = parent_bb_.lock())
126-
{
127-
parent->set(remapped_key, value);
128-
return;
129-
}
130-
}
131-
132103
// check local storage
133104
auto it = storage_.find(key);
105+
std::shared_ptr<Entry> entry;
134106
if (it != storage_.end())
135107
{
136-
const PortInfo& port_info = it->second.port_info;
137-
auto& previous_any = it->second.value;
138-
const auto previous_type = port_info.type();
108+
entry = it->second;
109+
}
110+
else
111+
{
112+
lock.unlock();
113+
entry = createEntryImpl(key, PortInfo());
114+
entry->value = Any(value);
115+
return;
116+
}
139117

140-
Any new_value(value);
118+
const PortInfo& port_info = entry->port_info;
119+
auto& previous_any = entry->value;
120+
const auto previous_type = port_info.type();
141121

142-
if (previous_type && *previous_type != typeid(T) &&
143-
*previous_type != new_value.type())
122+
Any new_value(value);
123+
124+
if (previous_type && *previous_type != typeid(T) &&
125+
*previous_type != new_value.type())
126+
{
127+
bool mismatching = true;
128+
if (std::is_constructible<StringView, T>::value)
144129
{
145-
bool mismatching = true;
146-
if (std::is_constructible<StringView, T>::value)
130+
Any any_from_string = port_info.parseString(value);
131+
if (any_from_string.empty() == false)
147132
{
148-
Any any_from_string = port_info.parseString(value);
149-
if (any_from_string.empty() == false)
150-
{
151-
mismatching = false;
152-
new_value = std::move(any_from_string);
153-
}
133+
mismatching = false;
134+
new_value = std::move(any_from_string);
154135
}
136+
}
155137

156-
if (mismatching)
157-
{
158-
debugMessage();
138+
if (mismatching)
139+
{
140+
debugMessage();
159141

160-
throw LogicError("Blackboard::set() failed: once declared, the type of a port "
161-
"shall not change. "
162-
"Declared type [",
163-
BT::demangle(previous_type), "] != current type [",
164-
BT::demangle(typeid(T)), "]");
165-
}
142+
throw LogicError("Blackboard::set() failed: once declared, the type of a port "
143+
"shall not change. Declared type [",
144+
BT::demangle(previous_type), "] != current type [",
145+
BT::demangle(typeid(T)), "]");
166146
}
167-
previous_any = std::move(new_value);
168147
}
169-
else
170-
{ // create for the first time without any info
171-
storage_.emplace(key, Entry(Any(value), PortInfo()));
172-
}
173-
return;
148+
previous_any = std::move(new_value);
174149
}
175150

176-
void setPortInfo(std::string key, const PortInfo& info);
177-
178151
const PortInfo* portInfo(const std::string& key);
179152

180153
void addSubtreeRemapping(StringView internal, StringView external);
@@ -197,6 +170,11 @@ class Blackboard
197170
return entry_mutex_;
198171
}
199172

173+
void createEntry(const std::string& key, const PortInfo& info)
174+
{
175+
createEntryImpl(key, info);
176+
}
177+
200178
private:
201179
struct Entry
202180
{
@@ -211,11 +189,15 @@ class Blackboard
211189
{}
212190
};
213191

192+
std::shared_ptr<Entry> createEntryImpl(const std::string& key, const PortInfo& info);
193+
214194
mutable std::mutex mutex_;
215195
mutable std::mutex entry_mutex_;
216-
std::unordered_map<std::string, Entry> storage_;
196+
std::unordered_map<std::string, std::shared_ptr<Entry>> storage_;
217197
std::weak_ptr<Blackboard> parent_bb_;
218198
std::unordered_map<std::string, std::string> internal_to_external_;
199+
200+
bool autoremapping_ = false;
219201
};
220202

221203
} // namespace BT

‎include/behaviortree_cpp_v3/tree_node.h

Lines changed: 21 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -258,24 +258,30 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const
258258

259259
std::unique_lock<std::mutex> entry_lock(config_.blackboard->entryMutex());
260260
const Any* val = config_.blackboard->getAny(static_cast<std::string>(remapped_key));
261-
if (val && val->empty() == false)
261+
262+
if(!val)
262263
{
263-
if (std::is_same<T, std::string>::value == false &&
264-
val->type() == typeid(std::string))
265-
{
266-
destination = convertFromString<T>(val->cast<std::string>());
267-
}
268-
else
269-
{
270-
destination = val->cast<T>();
271-
}
272-
return {};
264+
return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to "
265+
"find the port [", key,
266+
"] remapped to BB [", remapped_key, "]"));
267+
}
268+
269+
if(val->empty())
270+
{
271+
return nonstd::make_unexpected(StrCat("getInput() failed because the port [", key,
272+
"] remapped to BB [", remapped_key, "] was found,"
273+
"but its content was not initialized correctly"));
273274
}
274275

275-
return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to "
276-
"find the "
277-
"key [",
278-
key, "] remapped to [", remapped_key, "]"));
276+
if (!std::is_same<T, std::string>::value && val->type() == typeid(std::string))
277+
{
278+
destination = convertFromString<T>(val->cast<std::string>());
279+
}
280+
else
281+
{
282+
destination = val->cast<T>();
283+
}
284+
return {};
279285
}
280286
catch (std::exception& err)
281287
{

‎src/blackboard.cpp

Lines changed: 53 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -2,59 +2,17 @@
22

33
namespace BT
44
{
5-
void Blackboard::setPortInfo(std::string key, const PortInfo& info)
5+
void Blackboard::enableAutoRemapping(bool remapping)
66
{
7-
std::unique_lock<std::mutex> lock(mutex_);
8-
9-
if (auto parent = parent_bb_.lock())
10-
{
11-
auto remapping_it = internal_to_external_.find(key);
12-
if (remapping_it != internal_to_external_.end())
13-
{
14-
parent->setPortInfo(remapping_it->second, info);
15-
return;
16-
}
17-
}
18-
19-
auto it = storage_.find(key);
20-
if (it == storage_.end())
21-
{
22-
storage_.emplace(key, Entry(info));
23-
}
24-
else
25-
{
26-
auto old_type = it->second.port_info.type();
27-
if (old_type && *old_type != *info.type())
28-
{
29-
throw LogicError("Blackboard::set() failed: once declared, the type of a "
30-
"port shall "
31-
"not change. "
32-
"Declared type [",
33-
BT::demangle(old_type), "] != current type [",
34-
BT::demangle(info.type()), "]");
35-
}
36-
}
7+
autoremapping_ = remapping;
378
}
389

3910
const PortInfo* Blackboard::portInfo(const std::string& key)
4011
{
4112
std::unique_lock<std::mutex> lock(mutex_);
4213

43-
if (auto parent = parent_bb_.lock())
44-
{
45-
auto remapping_it = internal_to_external_.find(key);
46-
if (remapping_it != internal_to_external_.end())
47-
{
48-
return parent->portInfo(remapping_it->second);
49-
}
50-
}
51-
5214
auto it = storage_.find(key);
53-
if (it == storage_.end())
54-
{
55-
return nullptr;
56-
}
57-
return &(it->second.port_info);
15+
return (it == storage_.end()) ? nullptr : &(it->second->port_info);
5816
}
5917

6018
void Blackboard::addSubtreeRemapping(StringView internal, StringView external)
@@ -65,26 +23,29 @@ void Blackboard::addSubtreeRemapping(StringView internal, StringView external)
6523

6624
void Blackboard::debugMessage() const
6725
{
68-
for (const auto& entry_it : storage_)
26+
for (const auto& it: storage_)
6927
{
70-
auto port_type = entry_it.second.port_info.type();
28+
const auto& key = it.first;
29+
const auto& entry = it.second;
30+
31+
auto port_type = entry->port_info.type();
7132
if (!port_type)
7233
{
73-
port_type = &(entry_it.second.value.type());
34+
port_type = &(entry->value.type());
7435
}
7536

76-
std::cout << entry_it.first << " (" << demangle(port_type) << ") -> ";
37+
std::cout << key << " (" << demangle(port_type) << ") -> ";
7738

7839
if (auto parent = parent_bb_.lock())
7940
{
80-
auto remapping_it = internal_to_external_.find(entry_it.first);
41+
auto remapping_it = internal_to_external_.find(key);
8142
if (remapping_it != internal_to_external_.end())
8243
{
8344
std::cout << "remapped to parent [" << remapping_it->second << "]" << std::endl;
8445
continue;
8546
}
8647
}
87-
std::cout << ((entry_it.second.value.empty()) ? "empty" : "full") << std::endl;
48+
std::cout << ((entry->value.empty()) ? "empty" : "full") << std::endl;
8849
}
8950
}
9051

@@ -103,4 +64,45 @@ std::vector<StringView> Blackboard::getKeys() const
10364
return out;
10465
}
10566

67+
std::shared_ptr<Blackboard::Entry>
68+
Blackboard::createEntryImpl(const std::string &key, const PortInfo& info)
69+
{
70+
std::unique_lock<std::mutex> lock(mutex_);
71+
// This function might be called recursively, when we do remapping, because we move
72+
// to the top scope to find already existing entries
73+
74+
// search if exists already
75+
auto storage_it = storage_.find(key);
76+
if(storage_it != storage_.end())
77+
{
78+
return storage_it->second;
79+
}
80+
81+
std::shared_ptr<Entry> entry;
82+
83+
// manual remapping first
84+
auto remapping_it = internal_to_external_.find(key);
85+
if (remapping_it != internal_to_external_.end())
86+
{
87+
const auto& remapped_key = remapping_it->second;
88+
if (auto parent = parent_bb_.lock())
89+
{
90+
entry = parent->createEntryImpl(remapped_key, info);
91+
}
92+
}
93+
else if(autoremapping_)
94+
{
95+
if (auto parent = parent_bb_.lock())
96+
{
97+
entry = parent->createEntryImpl(key, info);
98+
}
99+
}
100+
else // not remapped, nor found. Create locally.
101+
{
102+
entry = std::make_shared<Entry>(info);
103+
}
104+
storage_.insert( {key, entry} );
105+
return entry;
106+
}
107+
106108
} // namespace BT

0 commit comments

Comments
 (0)