Skip to content

Commit 79e25b8

Browse files
facontidavidefacontidavide
authored
Merge pull request #583 from BehaviorTree/issue563
Issue563
2 parents da88d88 + 90d7bf9 commit 79e25b8

File tree

5 files changed

+176
-157
lines changed

5 files changed

+176
-157
lines changed

‎include/behaviortree_cpp_v3/blackboard.h

Lines changed: 50 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,55 @@ 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();
139-
108+
entry = it->second;
109+
}
110+
else
111+
{
140112
Any new_value(value);
113+
lock.unlock();
114+
entry = createEntryImpl(key, PortInfo(PortDirection::INOUT, new_value.type(), {}));
115+
entry->value = new_value;
116+
return;
117+
}
118+
119+
const PortInfo& port_info = entry->port_info;
120+
auto& previous_any = entry->value;
121+
const auto previous_type = port_info.type();
122+
123+
Any new_value(value);
141124

142-
if (previous_type && *previous_type != typeid(T) &&
143-
*previous_type != new_value.type())
125+
if (previous_type && *previous_type != typeid(T) &&
126+
*previous_type != new_value.type())
127+
{
128+
bool mismatching = true;
129+
if (std::is_constructible<StringView, T>::value)
144130
{
145-
bool mismatching = true;
146-
if (std::is_constructible<StringView, T>::value)
131+
Any any_from_string = port_info.parseString(value);
132+
if (any_from_string.empty() == false)
147133
{
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-
}
134+
mismatching = false;
135+
new_value = std::move(any_from_string);
154136
}
137+
}
155138

156-
if (mismatching)
157-
{
158-
debugMessage();
139+
if (mismatching)
140+
{
141+
debugMessage();
159142

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-
}
143+
throw LogicError("Blackboard::set() failed: once declared, the type of a port "
144+
"shall not change. Declared type [",
145+
BT::demangle(previous_type), "] != current type [",
146+
BT::demangle(typeid(T)), "]");
166147
}
167-
previous_any = std::move(new_value);
168-
}
169-
else
170-
{ // create for the first time without any info
171-
storage_.emplace(key, Entry(Any(value), PortInfo()));
172148
}
173-
return;
149+
previous_any = std::move(new_value);
174150
}
175151

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

180154
void addSubtreeRemapping(StringView internal, StringView external);
@@ -197,6 +171,11 @@ class Blackboard
197171
return entry_mutex_;
198172
}
199173

174+
void createEntry(const std::string& key, const PortInfo& info)
175+
{
176+
createEntryImpl(key, info);
177+
}
178+
200179
private:
201180
struct Entry
202181
{
@@ -211,11 +190,15 @@ class Blackboard
211190
{}
212191
};
213192

193+
std::shared_ptr<Entry> createEntryImpl(const std::string& key, const PortInfo& info);
194+
214195
mutable std::mutex mutex_;
215196
mutable std::mutex entry_mutex_;
216-
std::unordered_map<std::string, Entry> storage_;
197+
std::unordered_map<std::string, std::shared_ptr<Entry>> storage_;
217198
std::weak_ptr<Blackboard> parent_bb_;
218199
std::unordered_map<std::string, std::string> internal_to_external_;
200+
201+
bool autoremapping_ = false;
219202
};
220203

221204
} // 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: 61 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,53 @@ 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+
const auto old_type = storage_it->second->port_info.type();
79+
if (old_type && info.type() && old_type != info.type())
80+
{
81+
throw LogicError("Blackboard: once declared, the type of a port "
82+
"shall not change. Previously declared type [",
83+
BT::demangle(old_type), "] != new type [",
84+
BT::demangle(info.type()), "]");
85+
}
86+
return storage_it->second;
87+
}
88+
89+
std::shared_ptr<Entry> entry;
90+
91+
// manual remapping first
92+
auto remapping_it = internal_to_external_.find(key);
93+
if (remapping_it != internal_to_external_.end())
94+
{
95+
const auto& remapped_key = remapping_it->second;
96+
if (auto parent = parent_bb_.lock())
97+
{
98+
entry = parent->createEntryImpl(remapped_key, info);
99+
}
100+
}
101+
else if(autoremapping_)
102+
{
103+
if (auto parent = parent_bb_.lock())
104+
{
105+
entry = parent->createEntryImpl(key, info);
106+
}
107+
}
108+
else // not remapped, nor found. Create locally.
109+
{
110+
entry = std::make_shared<Entry>(info);
111+
}
112+
storage_.insert( {key, entry} );
113+
return entry;
114+
}
115+
106116
} // namespace BT

0 commit comments

Comments
 (0)