Skip to content

Commit 031312d

Browse files
dsobekdsobek
committed
Progress on subtree ports
1 parent 7236489 commit 031312d

File tree

4 files changed

+108
-89
lines changed

4 files changed

+108
-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: 87 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
@@ -644,6 +645,7 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
644645
const auto element_name = element->Name();
645646
const auto element_ID = element->Attribute("ID");
646647

648+
// TODO: Pull out this node type logic
647649
auto node_type = convertFromString<NodeType>(element_name);
648650
// name used by the factory
649651
std::string type_ID;
@@ -688,16 +690,20 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
688690

689691
PortsRemapping port_remap;
690692
NonPortAttributes other_attributes;
693+
// Only relevant for subtrees
694+
bool do_autoremap = false;
691695

696+
// Parse ports and validate them where we can.
692697
for(const XMLAttribute* att = element->FirstAttribute(); att; att = att->Next())
693698
{
694699
const std::string port_name = att->Name();
695-
const std::string port_value = att->Value();
700+
std::string port_value = att->Value();
696701
if(IsAllowedPortName(port_name))
697702
{
698-
const std::string port_name = att->Name();
699-
const std::string port_value = att->Value();
700-
703+
if(port_value == "{=}")
704+
{
705+
port_value = StrCat("{", port_name, "}");
706+
}
701707
if(manifest)
702708
{
703709
auto port_model_it = manifest->ports.find(port_name);
@@ -709,32 +715,34 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
709715
") but not in the providedPorts() of its "
710716
"registered node type."));
711717
}
712-
else
718+
719+
const auto& port_model = port_model_it->second;
720+
bool is_blacbkboard = port_value.size() >= 3 && port_value.front() == '{' &&
721+
port_value.back() == '}';
722+
// let's test already if conversion is possible
723+
if(!is_blacbkboard && port_model.converter() && port_model.isStronglyTyped())
713724
{
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())
725+
// This may throw
726+
try
719727
{
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-
}
728+
port_model.converter()(port_value);
729+
}
730+
catch(std::exception& ex)
731+
{
732+
auto msg =
733+
StrCat("The port with name \"", port_name, "\" and value \"", port_value,
734+
"\" can not be converted to ", port_model.typeName());
735+
throw LogicError(msg);
732736
}
733737
}
734738
}
735739

736740
port_remap[port_name] = port_value;
737741
}
742+
else if(node_type == NodeType::SUBTREE && port_name == "_autoremap")
743+
{
744+
do_autoremap = convertFromString<bool>(port_value);
745+
}
738746
else if(!IsReservedAttribute(port_name))
739747
{
740748
other_attributes[port_name] = port_value;
@@ -746,6 +754,7 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
746754
config.path = prefix_path + instance_name;
747755
config.uid = output_tree.getUID();
748756
config.manifest = manifest;
757+
config.auto_remapped = do_autoremap;
749758

750759
if(type_ID == instance_name)
751760
{
@@ -775,9 +784,49 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element,
775784
//---------------------------------------------
776785
TreeNode::Ptr new_node;
777786

787+
// TODO: in order to set the config at this point, we need the subtree model, which is parsed after this function call in recursivelyCreateSubtree
778788
if(node_type == NodeType::SUBTREE)
779789
{
780-
config.input_ports = port_remap;
790+
// check if this subtree has a model. If it does,
791+
// we want to check if all the mandatory ports were remapped and
792+
// add default ones, if necessary.
793+
auto subtree_model_it = subtree_models.find(type_ID);
794+
if(subtree_model_it != subtree_models.end())
795+
{
796+
const PortsList& subtree_model_ports = subtree_model_it->second.ports;
797+
// check if:
798+
// - remapping contains mandatory ports
799+
// - if any of these has default value
800+
for(const auto& [port_name, port_info] : subtree_model_ports)
801+
{
802+
auto it = port_remap.find(port_name);
803+
// don't override existing remapping
804+
if(it == port_remap.end() && !do_autoremap)
805+
{
806+
// remapping is not explicitly defined in the XML: use the model
807+
if(port_info.defaultValueString().empty())
808+
{
809+
auto msg = StrCat("In the <TreeNodesModel> the <Subtree ID=\"", type_ID,
810+
"\"> is defining a mandatory port called [", port_name,
811+
"], but you are not remapping it");
812+
throw RuntimeError(msg);
813+
}
814+
it = port_remap.insert({ port_name, port_info.defaultValueString() }).first;
815+
}
816+
// Include the ports in the node config
817+
if(port_info.direction() == PortDirection::INPUT ||
818+
port_info.direction() == PortDirection::INOUT)
819+
{
820+
config.input_ports[it->first] = it->second;
821+
}
822+
if(port_info.direction() == PortDirection::OUTPUT ||
823+
port_info.direction() == PortDirection::INOUT)
824+
{
825+
config.output_ports[it->first] = it->second;
826+
}
827+
}
828+
}
829+
781830
new_node =
782831
factory.instantiateTreeNode(instance_name, toStr(NodeType::SUBTREE), config);
783832
auto subtree_node = dynamic_cast<SubTreeNode*>(new_node.get());
@@ -933,7 +982,8 @@ void BT::XMLParser::PImpl::recursivelyCreateSubtree(const std::string& tree_ID,
933982
recursiveStep = [&](TreeNode::Ptr parent_node, Tree::Subtree::Ptr subtree,
934983
std::string prefix, const XMLElement* element) {
935984
// create the node
936-
auto node = createNodeFromXML(element, blackboard, parent_node, prefix, output_tree);
985+
TreeNode::Ptr node =
986+
createNodeFromXML(element, blackboard, parent_node, prefix, output_tree);
937987
subtree->nodes.push_back(node);
938988

939989
// common case: iterate through all children
@@ -949,76 +999,27 @@ void BT::XMLParser::PImpl::recursivelyCreateSubtree(const std::string& tree_ID,
949999
{
9501000
auto new_bb = Blackboard::create(blackboard);
9511001
const std::string subtree_ID = element->Attribute("ID");
952-
std::unordered_map<std::string, std::string> subtree_remapping;
953-
bool do_autoremap = false;
954-
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)
1002+
TreeNode::ConstPtr const_node = node;
1003+
const bool do_autoremap = const_node->config().auto_remapped;
1004+
1005+
// Populate the subtree's blackboard with it's port values.
1006+
PortsRemapping subtree_remapping = const_node->config().input_ports;
1007+
const PortsRemapping output_ports = const_node->config().output_ports;
1008+
subtree_remapping.insert(output_ports.begin(), output_ports.end());
1009+
for(const auto& [port_name, port_value] : subtree_remapping)
10091010
{
1010-
if(TreeNode::isBlackboardPointer(attr_value))
1011+
if(TreeNode::isBlackboardPointer(port_value))
10111012
{
10121013
// do remapping
1013-
StringView port_name = TreeNode::stripBlackboardPointer(attr_value);
1014-
new_bb->addSubtreeRemapping(attr_name, port_name);
1014+
StringView pointer_name = TreeNode::stripBlackboardPointer(port_value);
1015+
new_bb->addSubtreeRemapping(port_name, pointer_name);
10151016
}
10161017
else
10171018
{
10181019
// constant string: just set that constant value into the BB
10191020
// IMPORTANT: this must not be autoremapped!!!
10201021
new_bb->enableAutoRemapping(false);
1021-
new_bb->set(attr_name, static_cast<std::string>(attr_value));
1022+
new_bb->set(port_name, static_cast<std::string>(port_value));
10221023
new_bb->enableAutoRemapping(do_autoremap);
10231024
}
10241025
}

‎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: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
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"
@@ -586,6 +587,20 @@ TEST(SubTree, SubtreeModels)
586587

587588
BehaviorTreeFactory factory;
588589
auto tree = factory.createTreeFromText(xml_text);
590+
591+
const TreeNode* subtreeNode = nullptr;
592+
tree.applyVisitor([&subtreeNode](const TreeNode* node) {
593+
if(node->name() == "MySub")
594+
{
595+
subtreeNode = node;
596+
}
597+
});
598+
599+
ASSERT_NE(subtreeNode, nullptr);
600+
EXPECT_THAT(subtreeNode->config().input_ports,
601+
::testing::Contains(::testing::Pair("in_name", "{my_name}")));
602+
EXPECT_THAT(subtreeNode->config().output_ports,
603+
::testing::Contains(::testing::Pair("out_state", "{my_state}")));
589604
tree.tickWhileRunning();
590605
}
591606

0 commit comments

Comments
 (0)