datetime:2023/05/12 10:30
author:nzb
BT11：行为树内外的数据传输
行为树不是封闭的，对内有不同subtree、不同port间的数据传递，对外有调用方的状态或结果获取，毕竟很多情况调用方不会满足于只知道执行结果的成功与失败，还想知道为何失败等详细信息。
树内即ports之间
参考BehaviorTree.CPP/examples/t02_basic_ports.cpp 的示例。
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root">
<SaySomething message="start thinking..." />
<ThinkWhatToSay text="{the_answer}"/>
<SaySomething message="{the_answer}" />
<SaySomething2 message="SaySomething2 works too..." />
<SaySomething2 message="{the_answer}" />
</Sequence>
</BehaviorTree>
</root>
类ThinkWhatToSay有1个string类型的OutputPort，并会在tick()时向该port写入值。
class ThinkWhatToSay : public BT::SyncActionNode {
public:
BT::NodeStatus tick() override {
setOutput("text", "The answer is 42");
return BT::NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts() {
return {BT::OutputPort<std::string>("text")};
}
};
类SaySomething有1个string类型的InputPort，并会在tick()时从该port读取值。
class SaySomething : public BT::SyncActionNode {
public:
BT::NodeStatus tick() override {
auto msg = getInput<std::string>("message");
...
std::cout << "Robot says: " << msg.value() << std::endl;
return BT::NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts() {
return {BT::InputPort<std::string>("message")};
}
};
ThinkWhatToSay和SaySomething，通过所在树的blackboard的1个名称为"the_answer"的entry进行数据读写传输。entry可以传输的数据的类型， 由port
限定。我们把blackboard的数量和包含的entry的名称打印出来，size=1因为没有子树，且只有1个名为"the_answer"的entry。
std::cout << tree.blackboard_stack.size() << std::endl;
for(const auto str : tree.blackboard_stack[0]->getKeys()) {
std::cout << str << std::endl;
}
1
the_answer
Blackboard
类Blackboard（以下简称BB或bb）有3个重要的数据成员：BT中数据的保存依赖于storage_（Entry集合），树间的映射依赖于parent_和internal_to_external_。
// 保存了blackboard的所有entry的信息，包含entry所对应的port的实时值
std::unordered_map<std::string, Entry> storage_;
// 指向父blackboard（父树的blackboard）的指针
// 若不是nullptr，说明该tree被其他树引用了，是subtree
std::weak_ptr<Blackboard> parent_bb_;
// 保存了blackboard中向外（向父blackboard）重映射的port名称
std::unordered_map<std::string, std::string> internal_to_external_;
struct Entry {
Any value; // port的值
const PortInfo port_info; // port的其他信息
Entry(const PortInfo& info) : port_info(info) {}
Entry(Any&& other_any, const PortInfo& info)
: value(std::move(other_any)), port_info(info) {}
};
我们可以理解为：node的数据读写通过port，但数据是放在对应着该port的Entry中，树中所有nodes的数据整体放在blackboard的storage_中。 这是通过xml
中如下语句实现的，EntryName就是storage_中元素的第1项string，“{EntryName}”是1个blackboard pointer。
<NodeName PortName="{EntryName}"/>
上面的语句不涉及树之间的关系，所以对internal_to_external_没影响。
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence>
<SaySomething message="666" />
<ThinkWhatToSay text="{the_answer}"/>
<SaySomething message="{the_answer}" />
</Sequence>
</BehaviorTree>
</root>
以BehaviorTree.CPP/examples/t02_basic_ports.cpp中的树为例，如上。当树构建第1个SaySomething节点时，
在XMLParser::Pimpl::createNodeFromXML()中，会将pair{message，666}存入该node.config_.input_ports中。 其中，config_
是NodeConfiguration类型，input_ports是PortsRemapping类型，即unordered_map<string, string>类型。 因为“666
”是普通的字面字符串，不是blackboard pointer（不带花括号），就与blackboard无关，数据是静态的，node构建后就不会改变， 所以这个数据是存在node自身的数据结构中，当获取名为message
的port的值时，也不会去bb中查找。
struct NodeConfiguration {
Blackboard::Ptr blackboard;
PortsRemapping input_ports; // 输入port的映射关系
PortsRemapping output_ports; // 输出port的映射关系
};
当树构建ThinkWhatToSay节点时，会将pair{text，{the_answer}}存入该node.config_.input_ports中。 发现"{the_answer}"是bb pointer
，就会把pair{the_answer, Entry}存入所在树的bb的storage_。此时Entry还未赋值， 因为树构建时节点并未运行tick()，也就没有对port
数据的任何操作，仅仅定义了关系。这样，通过text port读写值， 就变成了对bb的名为the_answer的Entry的操作，这就是所谓的树中节点间的数据传输靠共享的blackboard。
当树构建第2个SaySomething节点时，会将pair{message，{the_answer}}存入该node.config_.input_ports中。 因为bb的storage_
中已经有名为the_answer的Entry了，无需再添加了，所以storage_和internal_to_external_不会有任何改变。
至此，ThinkWhatToSay通过text port向bb的the_answer entry写入值（setOutput()），而SaySomething通过message port从同一个bb
的the_answer entry读取值（getInput()），数据流和逻辑就一目了然了。
getInput()
// 获取名为key的port的值
template <typename T>
inline Result TreeNode::getInput(const std::string& key, T& destination) const {
auto remap_it = config_.input_ports.find(key);
// 既然是读值，那么就应该是input port，就应该在config_.input_ports中
if (remap_it == config_.input_ports.end()) {
return nonstd::make_unexpected(
StrCat("getInput() failed because NodeConfiguration::input_ports "
"does not contain the key: [", key, "]"));
}
auto remapped_res = getRemappedKey(key, remap_it->second);
try {
if (!remapped_res) {
// remapped_res空，说明remap_it->second目前只是普通的字面字符串
destination = convertFromString<T>(remap_it->second);
return {};
}
const auto& remapped_key = remapped_res.value();
// 既然remapped_key是本bb的一个entry对应的port的名称，那么本bb必须是有效的非空的
if (!config_.blackboard) {
return nonstd::make_unexpected(
"getInput() trying to access a Blackboard(BB) entry, but BB is invalid");
}
// 从本bb获取对应的entry的值，即port的值
const Any* val =
config_.blackboard->getAny(static_cast<std::string>(remapped_key));
if (val && val->empty() == false) {
// 做类型转换
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 {};
}
// 没有找到对应port的entry
return nonstd::make_unexpected(
StrCat("getInput() failed because it was unable to find the key [",
key, "] remapped to [", remapped_key, "]"));
} catch (std::exception& err) {
return nonstd::make_unexpected(err.what());
}
}
getInput(key)和setOutput(key)都是先去node的config_.input_ports或config_.output_ports中寻找匹配的key。找到后，得到其匹配的字符串str。
若str不是bb pointer（不带{}花括号），那就是字面字符串，就返回这个字符串，进行必要的类型转换。若str是bb pointer，得到bb entry名（去掉{}花括号），
最后调用config_.blackboard->getAny(EntryName)读取值，或者调用config_.blackboard->set(EntryName)
设置值，所谓“值”，就是storage_.Entry.value。
// 获取名为key的port的值
Any* getAny(const std::string& key) {
std::unique_lock<std::mutex> lock(mutex_);
// 如果父blackboard不为空，需要检查是否有向父blackboard的重映射
if (auto parent = parent_bb_.lock()) {
auto remapping_it = internal_to_external_.find(key);
// 找到了，说明存在向父blackboard的重映射
if (remapping_it != internal_to_external_.end()) {
// 从父blackboard获取对应port名为 remapping_it->second 的entry的值
return parent->getAny(remapping_it->second);
}
}
// 到这，说明父bb为空，或者名为key的port不存在重映射，那就从本bb获取值
auto it = storage_.find(key);
// 若找到了，就返回本bb中对应port名为key的entry的值
return (it == storage_.end()) ? nullptr : &(it->second.value);
}
setOutput()
// 设置名为key的port的值
template <typename T>
inline Result TreeNode::setOutput(const std::string& key, const T& value) {
if (!config_.blackboard) {
return nonstd::make_unexpected(
"setOutput() failed: trying to access a BB entry, but BB is invalid");
}
auto remap_it = config_.output_ports.find(key);
// 既然是写值，那么就应该是output port，就应该在config_.output_ports中
if (remap_it == config_.output_ports.end()) {
return nonstd::make_unexpected(
StrCat("setOutput() failed: NodeConfiguration::output_ports "
"does not contain the key: [", key, "]"));
}
StringView remapped_key = remap_it->second;
// 这种特殊情况先不管
if (remapped_key == "=") {
remapped_key = key;
}
// 如果是bb指针，就把{name}改为name，就是去掉花括号
if (isBlackboardPointer(remapped_key)) {
remapped_key = stripBlackboardPointer(remapped_key);
}
// 既然是写值，key一定对应着本bb的某个entry，从而使其他node可以通过bb共享这个数据
config_.blackboard->set(static_cast<std::string>(remapped_key), value);
return {};
}
output_port和input_port的不同在于，output_port一定会对应着bb的一个entry。因为node之所以有output_port，就是想通过它向外传值， 让其他node
可以获得。所以上面代码中，即便remapped_key不是bb pointer也会是一个EntryName，也要调用config_.blackboard->set(key, value)。
// 设置名为key的port的值
template <typename T>
void set(const std::string& key, const T& value) {
std::unique_lock<std::mutex> lock(mutex_);
auto it = storage_.find(key);
// 如果父blackboard不为空，需要检查是否有向父blackboard的重映射
if (auto parent = parent_bb_.lock()) {
auto remapping_it = internal_to_external_.find(key);
// 找到了，说明存在向父blackboard的重映射
if (remapping_it != internal_to_external_.end()) {
const auto& remapped_key = remapping_it->second;
// 本bb中没有对应port的entry
if (it == storage_.end()) {
// 检查父bb中是否有对应的entry
auto parent_info = parent->portInfo(remapped_key);
if (parent_info) {
// 从父bb中获取对应的entry的portinfo，保存到本bb的storage_中
storage_.insert({key, Entry(*parent_info)});
} else {
// 父bb中没有对应port的entry，在本bb的storage_中添加entry，绑定空白的portinfo
storage_.insert({key, Entry(PortInfo())});
}
}
// 向父bb的对应entry设置值
parent->set(remapped_key, value);
return;
}
}
// 到这，说明父bb为空，或者名为key的port不存在重映射
// 本bb有对应entry，检查数据类型是否匹配，并更新值
if (it != storage_.end()) {
...
} else {
// 本bb没有对应entry，就按输入值添加一个到storage_
storage_.emplace(key, Entry(Any(value), PortInfo()));
}
return;
}
SetBlackboard
SetBlackboard是一个比较特殊的节点，因为它可以直接向所在tree的blackboard或父bb写入值。其双向port“output_key”，对应着bb的一个entry。
class SetBlackboard : public SyncActionNode {
public:
SetBlackboard(const std::string& name, const NodeConfiguration& config)
: SyncActionNode(name, config) {
setRegistrationID("SetBlackboard");
}
static PortsList providedPorts() {
return {
InputPort("value",
"Value represented as a string. convertFromString must be "
"implemented."),
BidirectionalPort("output_key",
"Name of the blackboard entry where the value "
"should be written")};
}
private:
virtual BT::NodeStatus tick() override {
std::string key, value;
if (!getInput("output_key", key)) {
throw RuntimeError("missing port [output_key]");
}
if (!getInput("value", value)) {
throw RuntimeError("missing port [value]");
}
setOutput("output_key", value);
return NodeStatus::SUCCESS;
}
};
以 BehaviorTree.CPP/examples/t03_generic_ports.cpp 中的行为树为例，结合上述原理，当树构建SetBlackboard节点时， 该node.config_.input_ports
和node.config_.output_ports都会添加pair{output_key, OtherGoal}，因为output_key是INOUT port。 此时，BT的bb的storage_
中不会添加对应的entry。直到构建第2个PrintTarget节点时，bb的storage_中才会添加{OtherGoal, Entry}。
为什么代码中tick()是调用setOutput("output_key", value)，而不是setOutput(key, value)呢？这里key指通过getInput("output_key", key)
获得的值。 因为在构建SetBlackboard节点时，output_ports添加的是pair{output_key, xxx}，即所有的对应关系、传递线索，是以output_key为准的。
对应关系在树构建时就已经确定了，在节点运行tick()时是不会变的，所以tick()中key的值没有发挥作用。
当然，不考虑SubTreePlus（没研究），我认为将SetBlackboard节点的output_key port由INOUT改为仅OUT也是可以的，验证下来也是OK的。
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<CalculateGoal goal="{GoalPosition}"/>
<PrintTarget target="{GoalPosition}"/>
<SetBlackboard output_key="OtherGoal" value="-1;3"/>
<PrintTarget target="{OtherGoal}"/>
</Sequence>
</BehaviorTree>
</root>
subtree之间
参考 BehaviorTree.CPP/examples/t06_basic_ports.cpp 的示例。
<root main_tree_to_execute = "MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="main_sequence">
<SetBlackboard output_key="move_goal" value="1;2;3" />
<SubTree ID="MoveRobot" target="move_goal" output="move_result" />
<SaySomething message="{move_result}"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="MoveRobot">
<Fallback name="move_robot_main">
<SequenceStar>
<MoveBase goal="{target}"/>
<SetBlackboard output_key="output" value="mission accomplished" />
</SequenceStar>
<ForceFailure>
<SetBlackboard output_key="output" value="mission failed" />
</ForceFailure>
</Fallback>
</BehaviorTree>
</root>
MainTree中包含1个MoveRobot subtree，所以有2个blackboard，blackboard[0]是MainTree的（一定是最外层树的），具有move_result和move_goal
2个entry， 而MoveRobot具有output和target 2个entry。在树运行之后，使用debugMessage()可以打印树之间的ports映射如下。full表明对应port
已经被设置值，可以被外部读取。
move_result (std::string) -> full
move_goal (Pose2D) -> full
--------------
output (std::string) -> remapped to parent [move_result]
target (Pose2D) -> remapped to parent [move_goal]
如果我们调整下debugMessage()在树创建之后、运行之前，映射会变成什么样呢？
int main() {
...
auto tree = factory.createTreeFromText(xml_text);
// 在树创建之后、运行之前打印映射信息
std::cout << "--------------" << std::endl;
tree.blackboard_stack[0]->debugMessage();
std::cout << "--------------" << std::endl;
tree.blackboard_stack[1]->debugMessage();
std::cout << "--------------" << std::endl;
// auto p = tree.blackboard_stack[0]->get<Pose2D>("move_goal");
// std::cout << "get pose (" << p.x << "," << p.y << "," << p.theta << ")" << std::endl;
NodeStatus status = NodeStatus::RUNNING;
while (status == NodeStatus::RUNNING) {
status = tree.tickRoot();
SleepMS(1); // optional sleep to avoid "busy loops"
}
return 0;
}
结果如下，可见MainTree的2个entry都没有设置值（empty），且MoveRobot subtree也少了1个entry信息，因为此时树未运行SetBlackboard节点，
自然也就不会创建出output entry。若读取empty的entry，就会发生异常。
move_result (std::string) -> empty
move_goal (Pose2D) -> empty
--------------
target (Pose2D) -> remapped to parent [move_goal]
深入解析
以BehaviorTree.CPP/examples/t06_subtree_port_remapping.cpp中的行为树为例。
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<SetBlackboard output_key="move_goal" value="1;2;3"/>
<MoveRobot target="move_goal" output="move_result"/>
<SaySomething message="{move_result}"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="MoveRobot">
<Sequence>
<MoveBase goal="{target}"/>
<SetBlackboard output_key="output" value="666"/>
</Sequence>
</BehaviorTree>
</root>
当构建到MoveRobot节点时，识别到它是一个SubTreeNode。当__shared_blackboard=false时，会为该subtree创建一个独立的blackboard（称为子bb
），令其parent_bb_
成员指针指向父bb（父tree的bb）。并且会在子bb的internal_to_external_中添加重映射{target，move_goal} 和 {output，move_result}
。然后递归进入MoveRobot subtree的各节点的构造。
当构建到MoveBase节点时，识别到target是一个bb pointer，但是节点所在树的bb（即子bb）是刚创建的，其storage_容器是空的，此时会调用子bb->setPortInfo()
来添加一个Entry。因为子bb的parent_bb_不为空，就要检查子bb的internal_to_external_中是否存在target向外的映射。若无，只需在子bb的storage_
中添加名为target的Entry；若有，还要在父bb的storage_中添加名为move_goal的Entry。因为subtree的target映射到父树的move_goal。
void Blackboard::setPortInfo(std::string key, const PortInfo& info) {
std::unique_lock<std::mutex> lock(mutex_);
// 有父bb，需要检查是否有向父bb的重映射
if (auto parent = parent_bb_.lock()) {
auto remapping_it = internal_to_external_.find(key);
if (remapping_it != internal_to_external_.end()) {
// 有向父bb的重映射，向父bb传递portinfo
parent->setPortInfo(remapping_it->second, info);
}
}
// 到这，说明父bb为空，或者名为key的port不存在重映射
auto it = storage_.find(key);
if (it == storage_.end()) {
// 本bb无对应entry，使用输入的portinfo构造Entry并保存入storage_
storage_.insert({std::move(key), Entry(info)});
} else {
// 本bb有对应entry，检查数据类型是否匹配，无需更新portinfo，因为创建一次后就不会改变
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()), "]");
}
}
}
结尾有个小问题，上面是怎么识别到
MoveRobot是SubTreeNode呢？在树的构建过程中，
XMLParser::Pimpl::loadDocImpl()会统计xml语句中标签“BehaviorTree”的个数，并将其名称（树的ID）保存在XMLParser::Pimpl的成员变量tree_roots中。
XMLParser::Pimpl::createNodeFromXML()会检查node ID是否在tree_roots中。若在，就标记为subtree node，即type是SUBTREE。
树与调用方之间
本小节需要和上一小节结合起来看。
当debugMessage()打印key->full时，才可以在外部读取树的blackboard的entry的值。
while (status == NodeStatus::RUNNING) {
status = tree.tickRoot();
SleepMS(1); // optional sleep to avoid "busy loops"
}
// 添加在树运行后和return之间，此时entry的值是保持的。
// 如果添加在树运行之前，可能会因为port还未被设置而读取触发异常。
auto p = tree.blackboard_stack[0]->get<Pose2D>("move_goal");
std::cout << "get pose (" << p.x << "," << p.y << "," << p.theta << ")" << std::endl;
return 0;
运行上面的代码，我们会得到：
get pose (1,2,3) // 是XML中SetBlackboard的结果
当debugMessage()打印key->empty时， 读取值抛出异常，参考上一小节被注释的代码。
terminate called after throwing an instance of 'std::runtime_error'
what(): Any::cast failed because it is empty
当然，从外部设置entry的值并不受其是empty/full的影响。
Pose2D p, q;
p.x = 10, p.y = 11, p.theta = 3.14;
tree.blackboard_stack[0]->set<Pose2D>("move_goal", p);
tree.blackboard_stack[0]->get<Pose2D>("move_goal", q);
std::cout << "get pose (" << q.x << "," << q.y << "," << q.theta << ")" << std::endl;
运行上面的代码，我们会得到：
get pose (10,11,3.14)