diff --git a/include/behaviortree_cpp/actions/set_blackboard_node.h b/include/behaviortree_cpp/actions/set_blackboard_node.h index 9882cea6f..1a240c3db 100644 --- a/include/behaviortree_cpp/actions/set_blackboard_node.h +++ b/include/behaviortree_cpp/actions/set_blackboard_node.h @@ -61,13 +61,16 @@ class SetBlackboardNode : public SyncActionNode const std::string value_str = config().input_ports.at("value"); StringView stripped_key; + BT::Any out_value; + + std::shared_ptr dst_entry = + config().blackboard->getEntry(output_key); + if(isBlackboardPointer(value_str, &stripped_key)) { const auto input_key = std::string(stripped_key); std::shared_ptr src_entry = config().blackboard->getEntry(input_key); - std::shared_ptr dst_entry = - config().blackboard->getEntry(output_key); if(!src_entry) { @@ -78,13 +81,35 @@ class SetBlackboardNode : public SyncActionNode config().blackboard->createEntry(output_key, src_entry->info); dst_entry = config().blackboard->getEntry(output_key); } - dst_entry->value = src_entry->value; + + out_value = src_entry->value; } else { - config().blackboard->set(output_key, value_str); + out_value = BT::Any(value_str); + } + + if(out_value.empty()) + return NodeStatus::FAILURE; + + // avoid type issues when port is remapped: current implementation of the set might be a little bit problematic for initialized on the fly values + // this still does not attack math issues + if(dst_entry && dst_entry->info.type() != typeid(std::string) && out_value.isString()) + { + try + { + out_value = dst_entry->info.parseString(out_value.cast()); + } + catch(const std::exception& e) + { + throw LogicError("Can't convert string [", out_value.cast(), + "] to type [", BT::demangle(dst_entry->info.type()), + "]: ", e.what()); + } } + config().blackboard->set(output_key, out_value); + return NodeStatus::SUCCESS; } }; diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp index fcbc272a3..35be37dab 100644 --- a/tests/gtest_blackboard.cpp +++ b/tests/gtest_blackboard.cpp @@ -458,6 +458,29 @@ struct Point double y; }; +// Template specialization to converts a string to Point. +namespace BT +{ +template <> +[[nodiscard]] Point convertFromString(StringView str) +{ + // We expect real numbers separated by semicolons + auto parts = splitString(str, ';'); + if(parts.size() != 2) + { + throw RuntimeError("invalid input)"); + } + else + { + Point output{ 0.0, 0.0 }; + output.x = convertFromString(parts[0]); + output.y = convertFromString(parts[1]); + // std::cout << "Building a position 2d object " << output.x << "; " << output.y << "\n" << std::flush; + return output; + } +} +} // end namespace BT + TEST(BlackboardTest, SetBlackboard_Issue725) { BT::BehaviorTreeFactory factory; @@ -654,3 +677,180 @@ TEST(BlackboardTest, TimestampedInterface) ASSERT_EQ(stamp_opt->seq, 2); ASSERT_GE(stamp_opt->time.count(), nsec_before); } + +TEST(BlackboardTest, SetBlackboard_Upd_Ts_SeqId) +{ + BT::BehaviorTreeFactory factory; + + const std::string xml_text = R"( + + + +