Update setOutput<T> to convert vectors to vector<Any> and `getInp…

…ut<T>` to convert `vector<BT::Any>` type to `vector<T>` * Support vector<Any> -> vector<typename T::value_type> conversion Don't check port type alignment for vector<Any> * 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> * Update include/behaviortree_cpp/blackboard.h Co-authored-by: Nathan Brooks <[email protected]> * Fix formatting with pre-commit * Add unit test passing a vector through ports --------- Co-authored-by: Nathan Brooks <[email protected]>
PickNikRobotics · Dec 17, 2024 · 9336ead · 9336ead
1 parent 1fcb624
commit 9336ead
Show file tree

Hide file tree

Showing 5 changed files with 209 additions and 7 deletions.
diff --git 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,23 @@ 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 a demangled type string is a std::vector<..>
+inline bool isVector(const 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 +275,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
@@ -30,7 +30,6 @@
 
 namespace BT
 {
-
 /// This information is used mostly by the XMLParser.
 struct TreeNodeManifest
 {
@@ -521,6 +520,30 @@ 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>());
@@ -593,7 +616,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
@@ -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
@@ -786,8 +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));
-
-          if(port_type_mismatch && !string_input)
+          // 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 & !(previous_is_vector && new_is_vector_any))
           {
             blackboard->debugMessage();
 

diff --git a/tests/gtest_ports.cpp b/tests/gtest_ports.cpp
@@ -705,3 +705,134 @@ TEST(PortTest, DefaultWronglyOverriden)
   // This is correct
   ASSERT_NO_THROW(auto tree = factory.createTreeFromText(xml_txt_correct));
 }
+
+class OutputVectorStringNode : public SyncActionNode
+{
+public:
+  OutputVectorStringNode(const std::string& name, const NodeConfig& config)
+    : SyncActionNode(name, config)
+  {}
+  static PortsList providedPorts()
+  {
+    return { InputPort<std::string>("string1", "val1", "First string"),
+             InputPort<std::string>("string2", "val2", "Second string"),
+             OutputPort<std::vector<std::string>>("string_vector", "{string_vector}",
+                                                  "Vector of strings.") };
+  }
+
+  NodeStatus tick() override
+  {
+    auto string1 = getInput<std::string>("string1");
+    auto string2 = getInput<std::string>("string2");
+
+    std::vector<std::string> out = { string1.value(), string2.value() };
+    setOutput("string_vector", out);
+    return NodeStatus::SUCCESS;
+  }
+};
+
+class InputVectorStringNode : public SyncActionNode
+{
+public:
+  InputVectorStringNode(const std::string& name, const NodeConfig& config)
+    : SyncActionNode(name, config)
+  {}
+  static PortsList providedPorts()
+  {
+    return { InputPort<std::vector<std::string>>("string_vector", "{string_vector}",
+                                                 "Vector of strings.") };
+  }
+
+  NodeStatus tick() override
+  {
+    std::vector<std::string> expected_vec = { "val1", "val2" };
+    std::vector<std::string> actual_vec;
+
+    if(!getInput<std::vector<std::string>>("string_vector", actual_vec))
+    {
+      return NodeStatus::FAILURE;
+    }
+    if(expected_vec == actual_vec)
+    {
+      return NodeStatus::SUCCESS;
+    }
+    else
+    {
+      return NodeStatus::FAILURE;
+    }
+  }
+};
+
+class InputVectorDoubleNode : public SyncActionNode
+{
+public:
+  InputVectorDoubleNode(const std::string& name, const NodeConfig& config)
+    : SyncActionNode(name, config)
+  {}
+  static PortsList providedPorts()
+  {
+    return { InputPort<std::vector<double>>("double_vector", "{double_vector}",
+                                            "Vector of doubles.") };
+  }
+
+  NodeStatus tick() override
+  {
+    std::vector<double> expected_vec = { 1.0, 2.0 };
+    std::vector<double> actual_vec;
+
+    if(!getInput<std::vector<double>>("double_vector", actual_vec))
+    {
+      return NodeStatus::FAILURE;
+    }
+    if(expected_vec == actual_vec)
+    {
+      return NodeStatus::SUCCESS;
+    }
+    else
+    {
+      return NodeStatus::FAILURE;
+    }
+  }
+};
+
+TEST(PortTest, VectorAny)
+{
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<OutputVectorStringNode>("OutputVectorStringNode");
+  factory.registerNodeType<InputVectorStringNode>("InputVectorStringNode");
+  factory.registerNodeType<InputVectorDoubleNode>("InputVectorDoubleNode");
+
+  std::string xml_txt_good = R"(
+    <root BTCPP_format="4" >
+      <BehaviorTree>
+        <Sequence name="root_sequence">
+          <OutputVectorStringNode/>
+          <InputVectorStringNode/>
+        </Sequence>
+      </BehaviorTree>
+    </root>)";
+
+  std::string xml_txt_bad = R"(
+    <root BTCPP_format="4" >
+      <BehaviorTree>
+        <Sequence name="root_sequence">
+          <OutputVectorStringNode/>
+          <InputVectorDoubleNode double_vector="{string_vector}"/>
+        </Sequence>
+      </BehaviorTree>
+    </root>)";
+
+  // Test that setting and retrieving a vector<string> works.
+  BT::Tree tree;
+  ASSERT_NO_THROW(tree = factory.createTreeFromText(xml_txt_good));
+
+  BT::NodeStatus status;
+  ASSERT_NO_THROW(status = tree.tickOnce());
+  ASSERT_EQ(status, NodeStatus::SUCCESS);
+
+  // Test that setting a port as a vector<string> and attempting to retrie it as a vector<double> fails.
+  ASSERT_NO_THROW(tree = factory.createTreeFromText(xml_txt_bad));
+
+  ASSERT_NO_THROW(status = tree.tickOnce());
+  ASSERT_EQ(status, NodeStatus::FAILURE);
+}