Skip to content

Commit 2649291

Browse files
dsobekdsobek
authored
Merge pull request #10 from PickNikRobotics/fix-subtree-ports
Add subtree ports to NodeConfig
2 parents 52dd779 + 5596408 commit 2649291

File tree

4 files changed

+133
-89
lines changed

4 files changed

+133
-89
lines changed

‎include/behaviortree_cpp/tree_node.h

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,8 @@ struct NodeConfig
9191
PortsRemapping input_ports;
9292
// output ports
9393
PortsRemapping output_ports;
94+
// If missing port fields are automatically remapped (only relevant for subtrees).
95+
bool auto_remapped = false;
9496

9597
// Any other attributes found in the xml that are not parsed as ports
9698
// or built-in identifier (e.g. anything with a leading '_')
@@ -130,7 +132,8 @@ inline constexpr bool hasNodeFullCtor()
130132
class TreeNode
131133
{
132134
public:
133-
typedef std::shared_ptr<TreeNode> Ptr;
135+
using Ptr = std::shared_ptr<TreeNode>;
136+
using ConstPtr = std::shared_ptr<const TreeNode>;
134137

135138
/**
136139
* @brief TreeNode main constructor.

‎src/xml_parsing.cpp

Lines changed: 104 additions & 86 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <string>
2020
#include <typeindex>
2121
#include "behaviortree_cpp/basic_types.h"
22+
#include "behaviortree_cpp/utils/strcat.hpp"
2223

2324
#if defined(_MSVC_LANG) && !defined(__clang__)
2425
#define __bt_cplusplus (_MSC_VER == 1900 ? 201103L : _MSVC_LANG)
@@ -89,7 +90,7 @@ auto StrEqual = [](const char* str1, const char* str2) -> bool {
8990

9091
struct SubtreeModel
9192
{
92-
std::unordered_map<std::string, BT::PortInfo> ports;
93+
PortsList ports;
9394
};
9495

9596
struct XMLParser::PImpl
@@ -689,15 +690,17 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
689690
PortsRemapping port_remap;
690691
NonPortAttributes other_attributes;
691692

693+
// Parse ports and validate them where we can.
692694
for(const XMLAttribute* att = element->FirstAttribute(); att; att = att->Next())
693695
{
694696
const std::string port_name = att->Name();
695-
const std::string port_value = att->Value();
697+
std::string port_value = att->Value();
696698
if(IsAllowedPortName(port_name))
697699
{
698-
const std::string port_name = att->Name();
699-
const std::string port_value = att->Value();
700-
700+
if(port_value == "{=}")
701+
{
702+
port_value = StrCat("{", port_name, "}");
703+
}
701704
if(manifest)
702705
{
703706
auto port_model_it = manifest->ports.find(port_name);
@@ -709,26 +712,24 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
709712
") but not in the providedPorts() of its "
710713
"registered node type."));
711714
}
712-
else
715+
716+
const auto& port_model = port_model_it->second;
717+
bool is_blacbkboard = port_value.size() >= 3 && port_value.front() == '{' &&
718+
port_value.back() == '}';
719+
// let's test already if conversion is possible
720+
if(!is_blacbkboard && port_model.converter() && port_model.isStronglyTyped())
713721
{
714-
const auto& port_model = port_model_it->second;
715-
bool is_blacbkboard = port_value.size() >= 3 && port_value.front() == '{' &&
716-
port_value.back() == '}';
717-
// let's test already if conversion is possible
718-
if(!is_blacbkboard && port_model.converter() && port_model.isStronglyTyped())
722+
// This may throw
723+
try
719724
{
720-
// This may throw
721-
try
722-
{
723-
port_model.converter()(port_value);
724-
}
725-
catch(std::exception& ex)
726-
{
727-
auto msg = StrCat("The port with name \"", port_name, "\" and value \"",
728-
port_value, "\" can not be converted to ",
729-
port_model.typeName());
730-
throw LogicError(msg);
731-
}
725+
port_model.converter()(port_value);
726+
}
727+
catch(std::exception& ex)
728+
{
729+
auto msg =
730+
StrCat("The port with name \"", port_name, "\" and value \"", port_value,
731+
"\" can not be converted to ", port_model.typeName());
732+
throw LogicError(msg);
732733
}
733734
}
734735
}
@@ -741,11 +742,22 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
741742
}
742743
}
743744

745+
bool do_autoremap = false;
746+
if(node_type == NodeType::SUBTREE)
747+
{
748+
const XMLAttribute* auto_remap_ptr = element->FindAttribute("_autoremap");
749+
if(auto_remap_ptr != nullptr)
750+
{
751+
do_autoremap = convertFromString<bool>(auto_remap_ptr->Value());
752+
}
753+
}
754+
744755
NodeConfig config;
745756
config.blackboard = blackboard;
746757
config.path = prefix_path + instance_name;
747758
config.uid = output_tree.getUID();
748759
config.manifest = manifest;
760+
config.auto_remapped = do_autoremap;
749761

750762
if(type_ID == instance_name)
751763
{
@@ -777,7 +789,59 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
777789

778790
if(node_type == NodeType::SUBTREE)
779791
{
780-
config.input_ports = port_remap;
792+
// check if this subtree has a model. If it does,
793+
// we want to check if all the mandatory ports were remapped and
794+
// add default ones, if necessary.
795+
auto subtree_model_it = subtree_models.find(type_ID);
796+
if(subtree_model_it != subtree_models.end())
797+
{
798+
const PortsList& subtree_model_ports = subtree_model_it->second.ports;
799+
// check if:
800+
// - remapping contains mandatory ports
801+
// - if any of these has default value
802+
for(const auto& [port_name, port_info] : subtree_model_ports)
803+
{
804+
auto it = port_remap.find(port_name);
805+
// don't override existing remapping
806+
if(it == port_remap.end() && !do_autoremap)
807+
{
808+
// remapping is not explicitly defined in the XML: use the model
809+
if(port_info.defaultValueString().empty())
810+
{
811+
auto msg = StrCat("In the <TreeNodesModel> the <Subtree ID=\"", type_ID,
812+
"\"> is defining a mandatory port called [", port_name,
813+
"], but you are not remapping it");
814+
throw RuntimeError(msg);
815+
}
816+
port_remap.insert({ port_name, port_info.defaultValueString() });
817+
}
818+
}
819+
}
820+
// populate the node config
821+
for(const auto& [port_name, port_value] : port_remap)
822+
{
823+
auto direction = PortDirection::INPUT;
824+
if(subtree_model_it != subtree_models.end())
825+
{
826+
const PortsList& subtree_model_ports = subtree_model_it->second.ports;
827+
if(const auto& it = subtree_model_ports.find(port_name);
828+
it != subtree_model_ports.end())
829+
{
830+
direction = it->second.direction();
831+
}
832+
}
833+
834+
// Include the ports in the node config
835+
if(direction == PortDirection::INPUT || direction == PortDirection::INOUT)
836+
{
837+
config.input_ports[port_name] = port_value;
838+
}
839+
if(direction == PortDirection::OUTPUT || direction == PortDirection::INOUT)
840+
{
841+
config.output_ports[port_name] = port_value;
842+
}
843+
}
844+
781845
new_node =
782846
factory.instantiateTreeNode(instance_name, toStr(NodeType::SUBTREE), config);
783847
auto subtree_node = dynamic_cast<SubTreeNode*>(new_node.get());
@@ -933,7 +997,8 @@ void BT::XMLParser::PImpl::recursivelyCreateSubtree(const std::string& tree_ID,
933997
recursiveStep = [&](TreeNode::Ptr parent_node, Tree::Subtree::Ptr subtree,
934998
std::string prefix, const XMLElement* element) {
935999
// create the node
936-
auto node = createNodeFromXML(element, blackboard, parent_node, prefix, output_tree);
1000+
TreeNode::Ptr node =
1001+
createNodeFromXML(element, blackboard, parent_node, prefix, output_tree);
9371002
subtree->nodes.push_back(node);
9381003

9391004
// common case: iterate through all children
@@ -947,78 +1012,31 @@ void BT::XMLParser::PImpl::recursivelyCreateSubtree(const std::string& tree_ID,
9471012
}
9481013
else // special case: SubTreeNode
9491014
{
950-
auto new_bb = Blackboard::create(blackboard);
9511015
const std::string subtree_ID = element->Attribute("ID");
952-
std::unordered_map<std::string, std::string> subtree_remapping;
953-
bool do_autoremap = false;
1016+
TreeNode::ConstPtr const_node = node;
9541017

955-
for(auto attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next())
956-
{
957-
std::string attr_name = attr->Name();
958-
std::string attr_value = attr->Value();
959-
if(attr_value == "{=}")
960-
{
961-
attr_value = StrCat("{", attr_name, "}");
962-
}
963-
964-
if(attr_name == "_autoremap")
965-
{
966-
do_autoremap = convertFromString<bool>(attr_value);
967-
new_bb->enableAutoRemapping(do_autoremap);
968-
continue;
969-
}
970-
if(!IsAllowedPortName(attr->Name()))
971-
{
972-
continue;
973-
}
974-
subtree_remapping.insert({ attr_name, attr_value });
975-
}
976-
// check if this subtree has a model. If it does,
977-
// we want to check if all the mandatory ports were remapped and
978-
// add default ones, if necessary
979-
auto subtree_model_it = subtree_models.find(subtree_ID);
980-
if(subtree_model_it != subtree_models.end())
981-
{
982-
const auto& subtree_model_ports = subtree_model_it->second.ports;
983-
// check if:
984-
// - remapping contains mondatory ports
985-
// - if any of these has default value
986-
for(const auto& [port_name, port_info] : subtree_model_ports)
987-
{
988-
auto it = subtree_remapping.find(port_name);
989-
// don't override existing remapping
990-
if(it == subtree_remapping.end() && !do_autoremap)
991-
{
992-
// remapping is not explicitly defined in the XML: use the model
993-
if(port_info.defaultValueString().empty())
994-
{
995-
auto msg = StrCat("In the <TreeNodesModel> the <Subtree ID=\"", subtree_ID,
996-
"\"> is defining a mandatory port called [", port_name,
997-
"], but you are not remapping it");
998-
throw RuntimeError(msg);
999-
}
1000-
else
1001-
{
1002-
subtree_remapping.insert({ port_name, port_info.defaultValueString() });
1003-
}
1004-
}
1005-
}
1006-
}
1007-
1008-
for(const auto& [attr_name, attr_value] : subtree_remapping)
1018+
auto new_bb = Blackboard::create(blackboard);
1019+
const bool do_autoremap = const_node->config().auto_remapped;
1020+
new_bb->enableAutoRemapping(do_autoremap);
1021+
1022+
// Populate the subtree's blackboard with it's port values.
1023+
PortsRemapping subtree_remapping = const_node->config().input_ports;
1024+
const PortsRemapping& output_ports = const_node->config().output_ports;
1025+
subtree_remapping.insert(output_ports.begin(), output_ports.end());
1026+
for(const auto& [port_name, port_value] : subtree_remapping)
10091027
{
1010-
if(TreeNode::isBlackboardPointer(attr_value))
1028+
if(TreeNode::isBlackboardPointer(port_value))
10111029
{
10121030
// do remapping
1013-
StringView port_name = TreeNode::stripBlackboardPointer(attr_value);
1014-
new_bb->addSubtreeRemapping(attr_name, port_name);
1031+
StringView pointer_name = TreeNode::stripBlackboardPointer(port_value);
1032+
new_bb->addSubtreeRemapping(port_name, pointer_name);
10151033
}
10161034
else
10171035
{
10181036
// constant string: just set that constant value into the BB
10191037
// IMPORTANT: this must not be autoremapped!!!
10201038
new_bb->enableAutoRemapping(false);
1021-
new_bb->set(attr_name, static_cast<std::string>(attr_value));
1039+
new_bb->set(port_name, static_cast<std::string>(port_value));
10221040
new_bb->enableAutoRemapping(do_autoremap);
10231041
}
10241042
}

‎tests/CMakeLists.txt

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -40,9 +40,9 @@ set(TEST_DEPENDECIES
4040

4141
if(ament_cmake_FOUND AND BUILD_TESTING)
4242

43-
find_package(ament_cmake_gtest REQUIRED)
43+
find_package(ament_cmake_gmock REQUIRED)
4444

45-
ament_add_gtest(${BTCPP_LIBRARY}_test ${BT_TESTS})
45+
ament_add_gmock(${BTCPP_LIBRARY}_test ${BT_TESTS})
4646
target_link_libraries(${BTCPP_LIBRARY}_test
4747
${TEST_DEPENDECIES}
4848
${ament_LIBRARIES})

‎tests/gtest_subtree.cpp

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,15 @@
11
#include <gtest/gtest.h>
2+
#include <gmock/gmock-matchers.h>
23
#include "behaviortree_cpp/bt_factory.h"
34
#include "../sample_nodes/dummy_nodes.h"
45
#include "../sample_nodes/movebase_node.h"
6+
#include "behaviortree_cpp/exceptions.h"
7+
#include "behaviortree_cpp/xml_parsing.h"
58
#include "test_helper.hpp"
69

710
using namespace BT;
11+
using ::testing::Contains;
12+
using ::testing::Pair;
813

914
TEST(SubTree, SiblingPorts_Issue_72)
1015
{
@@ -516,6 +521,11 @@ class Assert : public BT::SyncActionNode
516521
private:
517522
virtual BT::NodeStatus tick() override
518523
{
524+
const auto& condition = getInput<bool>("condition");
525+
if(!condition.has_value())
526+
{
527+
throw RuntimeError("Missing input port 'condition'.");
528+
}
519529
if(getInput<bool>("condition").value())
520530
return BT::NodeStatus::SUCCESS;
521531
else
@@ -586,6 +596,19 @@ TEST(SubTree, SubtreeModels)
586596

587597
BehaviorTreeFactory factory;
588598
auto tree = factory.createTreeFromText(xml_text);
599+
600+
const TreeNode* subtreeNode = nullptr;
601+
tree.applyVisitor([&subtreeNode](const TreeNode* node) {
602+
if(node->name() == "MySub")
603+
{
604+
subtreeNode = node;
605+
}
606+
});
607+
608+
ASSERT_NE(subtreeNode, nullptr);
609+
EXPECT_THAT(subtreeNode->config().input_ports, Contains(Pair("in_name", "{my_name}")));
610+
EXPECT_THAT(subtreeNode->config().output_ports, Contains(Pair("out_state", "{my_"
611+
"state}")));
589612
tree.tickWhileRunning();
590613
}
591614

0 commit comments

Comments
 (0)