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 1/5] 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();
From f8493e56da135b4494a0347daed493cef16efb86 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Thu, 3 Oct 2024 08:48:31 -0600
Subject: [PATCH 2/5] 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..d254b58a4 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 a demangled type string is 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();
From 915732d54022984ba2ac906febbd8d8d117d67e2 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Thu, 24 Oct 2024 13:43:41 -0600
Subject: [PATCH 3/5] Update include/behaviortree_cpp/blackboard.h
Co-authored-by: Nathan Brooks <nbbrooks@gmail.com>
---
include/behaviortree_cpp/blackboard.h | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/include/behaviortree_cpp/blackboard.h b/include/behaviortree_cpp/blackboard.h
index d254b58a4..1fed49c9f 100644
--- a/include/behaviortree_cpp/blackboard.h
+++ b/include/behaviortree_cpp/blackboard.h
@@ -34,7 +34,7 @@ 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(std::string type_name)
+inline bool isVector(const std::string& type_name)
{
return std::regex_match(type_name, std::regex(R"(^std::vector<.*>$)"));
}
From 9ce8b86edda0d3f25ad5c030aa3fcd8037ad0080 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Wed, 6 Nov 2024 07:39:48 -0700
Subject: [PATCH 4/5] Fix formatting with pre-commit
---
include/behaviortree_cpp/blackboard.h | 8 ++++++--
include/behaviortree_cpp/tree_node.h | 18 +++++++++++-------
2 files changed, 17 insertions(+), 9 deletions(-)
diff --git a/include/behaviortree_cpp/blackboard.h b/include/behaviortree_cpp/blackboard.h
index 1fed49c9f..5189f148c 100644
--- a/include/behaviortree_cpp/blackboard.h
+++ b/include/behaviortree_cpp/blackboard.h
@@ -28,10 +28,14 @@ struct StampedValue
// Helper trait to check if templated type is a std::vector
template <typename T>
-struct is_vector : std::false_type {};
+struct is_vector : std::false_type
+{
+};
template <typename T, typename A>
-struct is_vector<std::vector<T, A>> : std::true_type {};
+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)
diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h
index 1b4dc0fdc..597572ea4 100644
--- a/include/behaviortree_cpp/tree_node.h
+++ b/include/behaviortree_cpp/tree_node.h
@@ -522,21 +522,25 @@ inline Expected<Timestamp> TreeNode::getInputStamped(const std::string& key,
{
// 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 constexpr(is_vector<T>::value)
{
- if (!std::is_same_v<T, std::vector<Any>> && any_value.type() == typeid(std::vector<Any>))
+ 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))
+ 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>."
+ 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>(); });
+ 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 };
}
}
@@ -618,7 +622,7 @@ inline Result TreeNode::setOutput(const std::string& key, const T& value)
// 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); });
+ [](const auto& element) { return BT::Any(element); });
config().blackboard->set(static_cast<std::string>(remapped_key), any_vec);
}
else
From 66079688543b16efc2485119b9195476ce4960b1 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Wed, 6 Nov 2024 08:22:05 -0700
Subject: [PATCH 5/5] Add unit test passing a vector through ports
---
tests/gtest_ports.cpp | 131 ++++++++++++++++++++++++++++++++++++++++++
1 file changed, 131 insertions(+)
diff --git a/tests/gtest_ports.cpp b/tests/gtest_ports.cpp
index 8ba750919..24a5dbd85 100644
--- 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);
+}