From e3b0cd7c2a8f1d6ed3c8b29d47d880c8982b796e Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Mon, 23 Sep 2024 19:04:34 -0700 Subject: [PATCH 1/2] Support vector -> vector conversion Don't check port type alignment for vector --- include/behaviortree_cpp/tree_node.h | 26 ++++++++++++++++++++++++++ src/xml_parsing.cpp | 4 +++- 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 537176519..1d36d3fd1 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -30,6 +30,12 @@ namespace BT { +// Helper trait to check if a type is a std::vector +template +struct is_vector : std::false_type {}; + +template +struct is_vector> : std::true_type {}; /// This information is used mostly by the XMLParser. struct TreeNodeManifest @@ -521,6 +527,26 @@ inline Expected TreeNode::getInputStamped(const std::string& key, if(!entry->value.empty()) { + // Support vector -> vector conversion. + // Only want to compile this path when T is a vector type. + if constexpr (is_vector::value) + { + if (!std::is_same_v> && any_value.type() == typeid(std::vector)) + { + // If the object was originally placed on the blackboard as a vector, attempt to unwrap the vector + // elements according to the templated type. + auto any_vec = any_value.cast>(); + if (!any_vec.empty() && any_vec.front().type() != typeid(typename T::value_type)) + { + return nonstd::make_unexpected("Invalid cast requested from vector to vector." + " Element type does not align."); + } + destination = T(); + std::transform(any_vec.begin(), any_vec.end(), std::back_inserter(destination), + [](Any &element) { return element.cast(); }); + return Timestamp{ entry->sequence_id, entry->stamp }; + } + } if(!std::is_same_v && any_value.isString()) { destination = parseString(any_value.cast()); diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index 2e950e4d9..96ffbd8d7 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -786,8 +786,10 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element, // special case related to convertFromString bool const string_input = (prev_info->type() == typeid(std::string)); + // special case related to unwrapping vector objects. + bool const vec_any_input = (prev_info->type() == typeid(std::vector)); - if(port_type_mismatch && !string_input) + if(port_type_mismatch && !string_input && !vec_any_input) { blackboard->debugMessage(); From 823f2a213c5749f58837b5a20dee55a618663c4d Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Thu, 3 Oct 2024 08:48:31 -0600 Subject: [PATCH 2/2] Convert vector to vector before placing on the blackboard Also update checks to allow mismatch when a port was declared as a vector and we have an input port that takes it in as a vector --- include/behaviortree_cpp/basic_types.h | 3 ++- include/behaviortree_cpp/blackboard.h | 7 ++++++- include/behaviortree_cpp/tree_node.h | 14 +++++++++++++- src/xml_parsing.cpp | 8 ++++++-- 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/include/behaviortree_cpp/basic_types.h b/include/behaviortree_cpp/basic_types.h index 244f4b2a3..15d83538b 100644 --- a/include/behaviortree_cpp/basic_types.h +++ b/include/behaviortree_cpp/basic_types.h @@ -375,7 +375,8 @@ class TypeInfo [[nodiscard]] bool isStronglyTyped() const { - return type_info_ != typeid(AnyTypeAllowed) && type_info_ != typeid(BT::Any); + return type_info_ != typeid(AnyTypeAllowed) && type_info_ != typeid(BT::Any) && + type_info_ != typeid(std::vector); } [[nodiscard]] const StringConverter& converter() const diff --git a/include/behaviortree_cpp/blackboard.h b/include/behaviortree_cpp/blackboard.h index 1c3aa96c6..87ed4c8da 100644 --- a/include/behaviortree_cpp/blackboard.h +++ b/include/behaviortree_cpp/blackboard.h @@ -257,8 +257,13 @@ inline void Blackboard::set(const std::string& key, const T& value) std::type_index previous_type = entry.info.type(); + // Allow mismatch if going from vector -> vector. + bool previous_is_vector = std::string(previous_type.name()).find("vector") != std::string::npos; + bool new_is_vector_any = new_value.type() == typeid(std::vector); + // check type mismatch - if(previous_type != std::type_index(typeid(T)) && previous_type != new_value.type()) + if(previous_type != std::type_index(typeid(T)) && previous_type != new_value.type() && + !(previous_is_vector && new_is_vector_any)) { bool mismatching = true; if(std::is_constructible::value) diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 1d36d3fd1..2535125ca 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -619,7 +619,19 @@ inline Result TreeNode::setOutput(const std::string& key, const T& value) } remapped_key = stripBlackboardPointer(remapped_key); - config().blackboard->set(static_cast(remapped_key), value); + + if constexpr(is_vector::value && !std::is_same_v>) + { + // If the object is a vector but not a vector, convert it to vector before placing it on the blackboard. + auto any_vec = std::vector(); + std::transform(value.begin(), value.end(), std::back_inserter(any_vec), + [](const auto &element) { return BT::Any(element); }); + config().blackboard->set(static_cast(remapped_key), any_vec); + } + else + { + config().blackboard->set(static_cast(remapped_key), value); + } return {}; } diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index 96ffbd8d7..053a1f0fc 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -786,10 +786,14 @@ TreeNode::Ptr XMLParser::PImpl::createNodeFromXML(const XMLElement* element, // special case related to convertFromString bool const string_input = (prev_info->type() == typeid(std::string)); - // special case related to unwrapping vector objects. + // special case related to unwrapping vector -> vector objects. bool const vec_any_input = (prev_info->type() == typeid(std::vector)); + // special case related to wrapping vector -> vector objects. + bool previous_is_vector = std::string(prev_info->type().name()).find("vector") != std::string::npos; + bool new_is_vector_any = port_info.type() == typeid(std::vector); - if(port_type_mismatch && !string_input && !vec_any_input) + if(port_type_mismatch && !string_input && + !vec_any_input & !(previous_is_vector && new_is_vector_any)) { blackboard->debugMessage();