diff --git a/include/behaviortree_cpp/blackboard.h b/include/behaviortree_cpp/blackboard.h index 1c3aa96c6..5189f148c 100644 --- a/include/behaviortree_cpp/blackboard.h +++ b/include/behaviortree_cpp/blackboard.h @@ -4,6 +4,7 @@ #include #include #include +#include #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 +struct is_vector : std::false_type +{ +}; + +template +struct is_vector> : 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. + 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); + // 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 537176519..597572ea4 100644 --- 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 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()); @@ -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(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/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::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. + bool previous_is_vector = BT::isVector(prev_type_demangled); + bool new_is_vector_any = info.type() == typeid(std::vector); + 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 2e950e4d9..83a94ee12 100644 --- 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 -> vector objects. + bool const vec_any_input = (prev_info->type() == typeid(std::vector)); + // special case related to wrapping vector -> vector 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); + + 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 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("string1", "val1", "First string"), + InputPort("string2", "val2", "Second string"), + OutputPort>("string_vector", "{string_vector}", + "Vector of strings.") }; + } + + NodeStatus tick() override + { + auto string1 = getInput("string1"); + auto string2 = getInput("string2"); + + std::vector 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>("string_vector", "{string_vector}", + "Vector of strings.") }; + } + + NodeStatus tick() override + { + std::vector expected_vec = { "val1", "val2" }; + std::vector actual_vec; + + if(!getInput>("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>("double_vector", "{double_vector}", + "Vector of doubles.") }; + } + + NodeStatus tick() override + { + std::vector expected_vec = { 1.0, 2.0 }; + std::vector actual_vec; + + if(!getInput>("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"); + factory.registerNodeType("InputVectorStringNode"); + factory.registerNodeType("InputVectorDoubleNode"); + + std::string xml_txt_good = R"( + + + + + + + + )"; + + std::string xml_txt_bad = R"( + + + + + + + + )"; + + // Test that setting and retrieving a vector 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 and attempting to retrie it as a vector fails. + ASSERT_NO_THROW(tree = factory.createTreeFromText(xml_txt_bad)); + + ASSERT_NO_THROW(status = tree.tickOnce()); + ASSERT_EQ(status, NodeStatus::FAILURE); +}