diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 8678f40a9..75ed0d063 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -91,6 +91,8 @@ struct NodeConfig PortsRemapping input_ports; // output ports PortsRemapping output_ports; + // If missing port fields are automatically remapped (only relevant for subtrees). + bool auto_remapped = false; // Any other attributes found in the xml that are not parsed as ports // or built-in identifier (e.g. anything with a leading '_') @@ -130,7 +132,8 @@ inline constexpr bool hasNodeFullCtor() class TreeNode { public: - typedef std::shared_ptr Ptr; + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; /** * @brief TreeNode main constructor. diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index c64ecde17..1f77eb3b0 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -19,6 +19,7 @@ #include #include #include "behaviortree_cpp/basic_types.h" +#include "behaviortree_cpp/utils/strcat.hpp" #if defined(_MSVC_LANG) && !defined(__clang__) #define __bt_cplusplus (_MSC_VER == 1900 ? 201103L : _MSVC_LANG) @@ -89,7 +90,7 @@ auto StrEqual = [](const char* str1, const char* str2) -> bool { struct SubtreeModel { - std::unordered_map ports; + PortsList ports; }; struct XMLParser::PImpl @@ -689,15 +690,17 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element, PortsRemapping port_remap; NonPortAttributes other_attributes; + // Parse ports and validate them where we can. for(const XMLAttribute* att = element->FirstAttribute(); att; att = att->Next()) { const std::string port_name = att->Name(); - const std::string port_value = att->Value(); + std::string port_value = att->Value(); if(IsAllowedPortName(port_name)) { - const std::string port_name = att->Name(); - const std::string port_value = att->Value(); - + if(port_value == "{=}") + { + port_value = StrCat("{", port_name, "}"); + } if(manifest) { auto port_model_it = manifest->ports.find(port_name); @@ -709,26 +712,24 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element, ") but not in the providedPorts() of its " "registered node type.")); } - else + + const auto& port_model = port_model_it->second; + bool is_blacbkboard = port_value.size() >= 3 && port_value.front() == '{' && + port_value.back() == '}'; + // let's test already if conversion is possible + if(!is_blacbkboard && port_model.converter() && port_model.isStronglyTyped()) { - const auto& port_model = port_model_it->second; - bool is_blacbkboard = port_value.size() >= 3 && port_value.front() == '{' && - port_value.back() == '}'; - // let's test already if conversion is possible - if(!is_blacbkboard && port_model.converter() && port_model.isStronglyTyped()) + // This may throw + try { - // This may throw - try - { - port_model.converter()(port_value); - } - catch(std::exception& ex) - { - auto msg = StrCat("The port with name \"", port_name, "\" and value \"", - port_value, "\" can not be converted to ", - port_model.typeName()); - throw LogicError(msg); - } + port_model.converter()(port_value); + } + catch(std::exception& ex) + { + auto msg = + StrCat("The port with name \"", port_name, "\" and value \"", port_value, + "\" can not be converted to ", port_model.typeName()); + throw LogicError(msg); } } } @@ -741,11 +742,22 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element, } } + bool do_autoremap = false; + if(node_type == NodeType::SUBTREE) + { + const XMLAttribute* auto_remap_ptr = element->FindAttribute("_autoremap"); + if(auto_remap_ptr != nullptr) + { + do_autoremap = convertFromString(auto_remap_ptr->Value()); + } + } + NodeConfig config; config.blackboard = blackboard; config.path = prefix_path + instance_name; config.uid = output_tree.getUID(); config.manifest = manifest; + config.auto_remapped = do_autoremap; if(type_ID == instance_name) { @@ -777,7 +789,59 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element, if(node_type == NodeType::SUBTREE) { - config.input_ports = port_remap; + // check if this subtree has a model. If it does, + // we want to check if all the mandatory ports were remapped and + // add default ones, if necessary. + auto subtree_model_it = subtree_models.find(type_ID); + if(subtree_model_it != subtree_models.end()) + { + const PortsList& subtree_model_ports = subtree_model_it->second.ports; + // check if: + // - remapping contains mandatory ports + // - if any of these has default value + for(const auto& [port_name, port_info] : subtree_model_ports) + { + auto it = port_remap.find(port_name); + // don't override existing remapping + if(it == port_remap.end() && !do_autoremap) + { + // remapping is not explicitly defined in the XML: use the model + if(port_info.defaultValueString().empty()) + { + auto msg = StrCat("In the the is defining a mandatory port called [", port_name, + "], but you are not remapping it"); + throw RuntimeError(msg); + } + port_remap.insert({ port_name, port_info.defaultValueString() }); + } + } + } + // populate the node config + for(const auto& [port_name, port_value] : port_remap) + { + auto direction = PortDirection::INPUT; + if(subtree_model_it != subtree_models.end()) + { + const PortsList& subtree_model_ports = subtree_model_it->second.ports; + if(const auto& it = subtree_model_ports.find(port_name); + it != subtree_model_ports.end()) + { + direction = it->second.direction(); + } + } + + // Include the ports in the node config + if(direction == PortDirection::INPUT || direction == PortDirection::INOUT) + { + config.input_ports[port_name] = port_value; + } + if(direction == PortDirection::OUTPUT || direction == PortDirection::INOUT) + { + config.output_ports[port_name] = port_value; + } + } + new_node = factory.instantiateTreeNode(instance_name, toStr(NodeType::SUBTREE), config); auto subtree_node = dynamic_cast(new_node.get()); @@ -933,7 +997,8 @@ void BT::XMLParser::PImpl::recursivelyCreateSubtree(const std::string& tree_ID, recursiveStep = [&](TreeNode::Ptr parent_node, Tree::Subtree::Ptr subtree, std::string prefix, const XMLElement* element) { // create the node - auto node = createNodeFromXML(element, blackboard, parent_node, prefix, output_tree); + TreeNode::Ptr node = + createNodeFromXML(element, blackboard, parent_node, prefix, output_tree); subtree->nodes.push_back(node); // common case: iterate through all children @@ -947,78 +1012,31 @@ void BT::XMLParser::PImpl::recursivelyCreateSubtree(const std::string& tree_ID, } else // special case: SubTreeNode { - auto new_bb = Blackboard::create(blackboard); const std::string subtree_ID = element->Attribute("ID"); - std::unordered_map subtree_remapping; - bool do_autoremap = false; + TreeNode::ConstPtr const_node = node; - for(auto attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) - { - std::string attr_name = attr->Name(); - std::string attr_value = attr->Value(); - if(attr_value == "{=}") - { - attr_value = StrCat("{", attr_name, "}"); - } - - if(attr_name == "_autoremap") - { - do_autoremap = convertFromString(attr_value); - new_bb->enableAutoRemapping(do_autoremap); - continue; - } - if(!IsAllowedPortName(attr->Name())) - { - continue; - } - subtree_remapping.insert({ attr_name, attr_value }); - } - // check if this subtree has a model. If it does, - // we want to check if all the mandatory ports were remapped and - // add default ones, if necessary - auto subtree_model_it = subtree_models.find(subtree_ID); - if(subtree_model_it != subtree_models.end()) - { - const auto& subtree_model_ports = subtree_model_it->second.ports; - // check if: - // - remapping contains mondatory ports - // - if any of these has default value - for(const auto& [port_name, port_info] : subtree_model_ports) - { - auto it = subtree_remapping.find(port_name); - // don't override existing remapping - if(it == subtree_remapping.end() && !do_autoremap) - { - // remapping is not explicitly defined in the XML: use the model - if(port_info.defaultValueString().empty()) - { - auto msg = StrCat("In the the is defining a mandatory port called [", port_name, - "], but you are not remapping it"); - throw RuntimeError(msg); - } - else - { - subtree_remapping.insert({ port_name, port_info.defaultValueString() }); - } - } - } - } - - for(const auto& [attr_name, attr_value] : subtree_remapping) + auto new_bb = Blackboard::create(blackboard); + const bool do_autoremap = const_node->config().auto_remapped; + new_bb->enableAutoRemapping(do_autoremap); + + // Populate the subtree's blackboard with it's port values. + PortsRemapping subtree_remapping = const_node->config().input_ports; + const PortsRemapping& output_ports = const_node->config().output_ports; + subtree_remapping.insert(output_ports.begin(), output_ports.end()); + for(const auto& [port_name, port_value] : subtree_remapping) { - if(TreeNode::isBlackboardPointer(attr_value)) + if(TreeNode::isBlackboardPointer(port_value)) { // do remapping - StringView port_name = TreeNode::stripBlackboardPointer(attr_value); - new_bb->addSubtreeRemapping(attr_name, port_name); + StringView pointer_name = TreeNode::stripBlackboardPointer(port_value); + new_bb->addSubtreeRemapping(port_name, pointer_name); } else { // constant string: just set that constant value into the BB // IMPORTANT: this must not be autoremapped!!! new_bb->enableAutoRemapping(false); - new_bb->set(attr_name, static_cast(attr_value)); + new_bb->set(port_name, static_cast(port_value)); new_bb->enableAutoRemapping(do_autoremap); } } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 738f352e8..261c81f39 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -40,9 +40,9 @@ set(TEST_DEPENDECIES if(ament_cmake_FOUND AND BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) - ament_add_gtest(${BTCPP_LIBRARY}_test ${BT_TESTS}) + ament_add_gmock(${BTCPP_LIBRARY}_test ${BT_TESTS}) target_link_libraries(${BTCPP_LIBRARY}_test ${TEST_DEPENDECIES} ${ament_LIBRARIES}) diff --git a/tests/gtest_subtree.cpp b/tests/gtest_subtree.cpp index 6402bafba..e797a03a8 100644 --- a/tests/gtest_subtree.cpp +++ b/tests/gtest_subtree.cpp @@ -1,10 +1,15 @@ #include +#include #include "behaviortree_cpp/bt_factory.h" #include "../sample_nodes/dummy_nodes.h" #include "../sample_nodes/movebase_node.h" +#include "behaviortree_cpp/exceptions.h" +#include "behaviortree_cpp/xml_parsing.h" #include "test_helper.hpp" using namespace BT; +using ::testing::Contains; +using ::testing::Pair; TEST(SubTree, SiblingPorts_Issue_72) { @@ -516,6 +521,11 @@ class Assert : public BT::SyncActionNode private: virtual BT::NodeStatus tick() override { + const auto& condition = getInput("condition"); + if(!condition.has_value()) + { + throw RuntimeError("Missing input port 'condition'."); + } if(getInput("condition").value()) return BT::NodeStatus::SUCCESS; else @@ -586,6 +596,19 @@ TEST(SubTree, SubtreeModels) BehaviorTreeFactory factory; auto tree = factory.createTreeFromText(xml_text); + + const TreeNode* subtreeNode = nullptr; + tree.applyVisitor([&subtreeNode](const TreeNode* node) { + if(node->name() == "MySub") + { + subtreeNode = node; + } + }); + + ASSERT_NE(subtreeNode, nullptr); + EXPECT_THAT(subtreeNode->config().input_ports, Contains(Pair("in_name", "{my_name}"))); + EXPECT_THAT(subtreeNode->config().output_ports, Contains(Pair("out_state", "{my_" + "state}"))); tree.tickWhileRunning(); }