From e3b0cd7c2a8f1d6ed3c8b29d47d880c8982b796e Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Mon, 23 Sep 2024 19:04:34 -0700
Subject: [PATCH] Support vector<Any> -> vector<typename T::value_type>
 conversion

Don't check port type alignment for vector<Any>
---
 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 <typename T>
+struct is_vector : std::false_type {};
+
+template <typename T, typename A>
+struct is_vector<std::vector<T, A>> : std::true_type {};
 
 /// This information is used mostly by the XMLParser.
 struct TreeNodeManifest
@@ -521,6 +527,26 @@ inline Expected<Timestamp> TreeNode::getInputStamped(const std::string& key,
 
       if(!entry->value.empty())
       {
+        // Support vector<Any> -> vector<typename T::value_type> conversion.
+        // Only want to compile this path when T is a vector type.
+        if constexpr (is_vector<T>::value)
+        {
+          if (!std::is_same_v<T, std::vector<Any>> && any_value.type() == typeid(std::vector<Any>))
+          {
+            // If the object was originally placed on the blackboard as a vector<Any>, attempt to unwrap the vector
+            // elements according to the templated type.
+            auto any_vec = any_value.cast<std::vector<Any>>();
+            if (!any_vec.empty() && any_vec.front().type() != typeid(typename T::value_type))
+            {
+              return nonstd::make_unexpected("Invalid cast requested from vector<Any> to vector<typename T::value_type>."
+                                             " Element type does not align.");
+            }
+            destination = T();
+            std::transform(any_vec.begin(), any_vec.end(), std::back_inserter(destination),
+                           [](Any &element) { return element.cast<typename T::value_type>(); });
+            return Timestamp{ entry->sequence_id, entry->stamp };
+          }
+        }
         if(!std::is_same_v<T, std::string> && any_value.isString())
         {
           destination = parseString<T>(any_value.cast<std::string>());
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<Any> objects.
+          bool const vec_any_input = (prev_info->type() == typeid(std::vector<Any>));
 
-          if(port_type_mismatch && !string_input)
+          if(port_type_mismatch && !string_input && !vec_any_input)
           {
             blackboard->debugMessage();