From 7d0219e75058fb8c2e7ae34642dc4fbb8dd37540 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Thu, 3 Oct 2024 08:48:31 -0600
Subject: [PATCH] Convert vector to vector<Any> before placing on the
 blackboard

Also update checks to allow mismatch when a port was declared as a
vector<T> and we have an input port that takes it in as a vector<Any>
---
 include/behaviortree_cpp/blackboard.h | 22 +++++++++++++++++++++-
 include/behaviortree_cpp/tree_node.h  | 21 +++++++++++++--------
 src/blackboard.cpp                    |  9 +++++++--
 src/xml_parsing.cpp                   |  9 +++++++--
 4 files changed, 48 insertions(+), 13 deletions(-)

diff --git a/include/behaviortree_cpp/blackboard.h b/include/behaviortree_cpp/blackboard.h
index 1c3aa96c6..03b35335b 100644
--- a/include/behaviortree_cpp/blackboard.h
+++ b/include/behaviortree_cpp/blackboard.h
@@ -4,6 +4,7 @@
 #include <memory>
 #include <unordered_map>
 #include <mutex>
+#include <regex>
 
 #include "behaviortree_cpp/basic_types.h"
 #include "behaviortree_cpp/contrib/json.hpp"
@@ -25,6 +26,19 @@ struct StampedValue
   Timestamp stamp;
 };
 
+// Helper trait to check if templated 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 {};
+
+// Helper function to check if BT::Any object holds a std::vector
+inline bool isVector(std::string type_name)
+{
+  return std::regex_match(type_name, std::regex(R"(^std::vector<.*>$)"));
+}
+
 /**
  * @brief The Blackboard is the mechanism used by BehaviorTrees to exchange
  * typed data.
@@ -257,8 +271,14 @@ 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<Any>.
+    auto prev_type_demangled = BT::demangle(entry.value.type());
+    bool previous_is_vector = BT::isVector(prev_type_demangled);
+    bool new_is_vector_any = new_value.type() == typeid(std::vector<Any>);
+
     // 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<StringView, T>::value)
diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h
index 1d36d3fd1..1b4dc0fdc 100644
--- a/include/behaviortree_cpp/tree_node.h
+++ b/include/behaviortree_cpp/tree_node.h
@@ -30,13 +30,6 @@
 
 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
 {
@@ -619,7 +612,19 @@ inline Result TreeNode::setOutput(const std::string& key, const T& value)
   }
 
   remapped_key = stripBlackboardPointer(remapped_key);
-  config().blackboard->set(static_cast<std::string>(remapped_key), value);
+
+  if constexpr(is_vector<T>::value && !std::is_same_v<T, std::vector<Any>>)
+  {
+    // If the object is a vector but not a vector<Any>, convert it to vector<Any> before placing it on the blackboard.
+    auto any_vec = std::vector<Any>();
+    std::transform(value.begin(), value.end(), std::back_inserter(any_vec),
+                   [](const auto &element) { return BT::Any(element); });
+    config().blackboard->set(static_cast<std::string>(remapped_key), any_vec);
+  }
+  else
+  {
+    config().blackboard->set(static_cast<std::string>(remapped_key), value);
+  }
 
   return {};
 }
diff --git a/src/blackboard.cpp b/src/blackboard.cpp
index 0f1f304db..ad1ea99ff 100644
--- a/src/blackboard.cpp
+++ b/src/blackboard.cpp
@@ -217,13 +217,18 @@ std::shared_ptr<Blackboard::Entry> Blackboard::createEntryImpl(const std::string
   if(storage_it != storage_.end())
   {
     const auto& prev_info = storage_it->second->info;
+    auto prev_type_demangled = BT::demangle(prev_info.type());
+    // Allow mismatch if going from vector -> vector<Any>.
+    bool previous_is_vector = BT::isVector(prev_type_demangled);
+    bool new_is_vector_any = info.type() == typeid(std::vector<Any>);
+
     if(prev_info.type() != info.type() && prev_info.isStronglyTyped() &&
-       info.isStronglyTyped())
+       info.isStronglyTyped() && !(previous_is_vector && new_is_vector_any))
     {
       auto msg = StrCat("Blackboard entry [", key,
                         "]: once declared, the type of a port"
                         " shall not change. Previously declared type [",
-                        BT::demangle(prev_info.type()), "], current type [",
+                        prev_type_demangled, "], current type [",
                         BT::demangle(info.type()), "]");
 
       throw LogicError(msg);
diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp
index 96ffbd8d7..83a94ee12 100644
--- a/src/xml_parsing.cpp
+++ b/src/xml_parsing.cpp
@@ -786,10 +786,15 @@ 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.
+          // special case related to unwrapping vector<Any> -> vector<T> objects.
           bool const vec_any_input = (prev_info->type() == typeid(std::vector<Any>));
+          // special case related to wrapping vector<T> -> vector<Any> objects.
+          auto prev_type_demangled = demangle(prev_info->type());
+          bool previous_is_vector = BT::isVector(prev_type_demangled);
+          bool new_is_vector_any = port_info.type() == typeid(std::vector<Any>);
 
-          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();