diff --git a/include/behaviortree_cpp/basic_types.h b/include/behaviortree_cpp/basic_types.h index 2731f1974..0c8f56e04 100644 --- a/include/behaviortree_cpp/basic_types.h +++ b/include/behaviortree_cpp/basic_types.h @@ -86,6 +86,43 @@ using KeyValueVector = std::vector>; template using Expected = nonstd::expected; +/** + * @brief Machine-readable cause for why an input port could not be read. + * + * The plain string error returned by TreeNode::getInputStamped() is fine for + * console messages, but callers that want to react differently to different + * failure modes (for example, "the user wired a blackboard key but forgot to + * populate it" versus "the port is simply unwired and the behavior should + * fall back to its default") cannot tell those cases apart from a string. + * + * TreeNode::getInputStampedWithDiagnostic() returns this enum alongside the + * original human-readable message so upstream code can make that distinction. + */ +enum class PortError : uint8_t +{ + ManifestMissing, ///< config().manifest is null and the port has no XML entry. + ManifestKeyMissing, ///< The key is not in the manifest and not in the XML. + NoDefaultNoWiring, ///< The manifest has the key but no default value and the XML did not wire it. + BlackboardKeyNotFound, ///< The port is wired to {foo} but the blackboard has no entry called foo. + BlackboardEntryEmpty, ///< The blackboard entry exists but its value is empty. + InvalidBlackboard, ///< config().blackboard is null. + ConversionFailed, ///< parseString() threw while converting a string to T. + CastFailed, ///< Any::cast() or a vector element cast threw. +}; + +/** + * @brief Diagnostic error returned by TreeNode::getInputStampedWithDiagnostic(). + * + * `code` carries the machine-readable cause; `message` preserves the same + * human-readable text the non-diagnostic getInputStamped() overload would + * have returned so existing log output is unchanged. + */ +struct PortInputError +{ + PortError code; + std::string message; +}; + struct AnyTypeAllowed { }; diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 1dc73986d..3d347522c 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -263,6 +263,20 @@ class TreeNode [[nodiscard]] Expected getInputStamped(const std::string& key, T& destination) const; + /** + * @brief Same as getInputStamped(key, destination) but returns a structured + * PortInputError on failure, so callers can distinguish the different reasons + * a port read can fail (unwired manifest key vs. blackboard key missing vs. + * conversion failure, etc.). The `.message` field preserves the exact + * human-readable text getInputStamped() would have returned. + * + * @param key the name of the port. + * @param destination reference to the object where the value should be stored + */ + template + [[nodiscard]] nonstd::expected + getInputStampedWithDiagnostic(const std::string& key, T& destination) const; + /** Same as bool getInput(const std::string& key, T& destination) * but using optional. * @@ -296,6 +310,28 @@ class TreeNode } } + /** Value-returning convenience for getInputStampedWithDiagnostic(). + * + * Returns the port value directly on success, or a nonstd::expected carrying + * the structured PortInputError on failure. Use this when you only need the + * value (not the Timestamp) but still want to distinguish the failure + * reasons the diagnostic variant exposes. + * + * @param key the name of the port. + */ + template + [[nodiscard]] nonstd::expected + getInputWithDiagnostic(const std::string& key) const + { + T out{}; + auto res = getInputStampedWithDiagnostic(key, out); + if(res) + { + return out; + } + return nonstd::make_unexpected(res.error()); + } + /** * @brief setOutput modifies the content of an Output port * @param key the name of the port. @@ -450,8 +486,8 @@ T TreeNode::parseString(const std::string& str) const } template -inline Expected TreeNode::getInputStamped(const std::string& key, - T& destination) const +inline nonstd::expected +TreeNode::getInputStampedWithDiagnostic(const std::string& key, T& destination) const { std::string port_value_str; @@ -462,10 +498,11 @@ inline Expected TreeNode::getInputStamped(const std::string& key, } else if(!config().manifest) { - return nonstd::make_unexpected(StrCat("getInput() of node '", fullPath(), - "' failed because the manifest is " - "nullptr (WTF?) and the key: [", - key, "] is missing")); + return nonstd::make_unexpected(PortInputError{ + PortError::ManifestMissing, StrCat("getInput() of node '", fullPath(), + "' failed because the manifest is " + "nullptr (WTF?) and the key: [", + key, "] is missing") }); } else { @@ -473,19 +510,21 @@ inline Expected TreeNode::getInputStamped(const std::string& key, auto port_manifest_it = config().manifest->ports.find(key); if(port_manifest_it == config().manifest->ports.end()) { - return nonstd::make_unexpected(StrCat("getInput() of node '", fullPath(), - "' failed because the manifest doesn't " - "contain the key: [", - key, "]")); + return nonstd::make_unexpected(PortInputError{ + PortError::ManifestKeyMissing, StrCat("getInput() of node '", fullPath(), + "' failed because the manifest doesn't " + "contain the key: [", + key, "]") }); } const auto& port_info = port_manifest_it->second; // there is a default value if(port_info.defaultValue().empty()) { - return nonstd::make_unexpected(StrCat("getInput() of node '", fullPath(), - "' failed because nor the manifest or the " - "XML contain the key: [", - key, "]")); + return nonstd::make_unexpected(PortInputError{ + PortError::NoDefaultNoWiring, StrCat("getInput() of node '", fullPath(), + "' failed because nor the manifest or the " + "XML contain the key: [", + key, "]") }); } if(port_info.defaultValue().isString()) { @@ -510,7 +549,8 @@ inline Expected TreeNode::getInputStamped(const std::string& key, } catch(std::exception& ex) { - return nonstd::make_unexpected(StrCat("getInput(): ", ex.what())); + return nonstd::make_unexpected(PortInputError{ + PortError::ConversionFailed, StrCat("getInput(): ", ex.what()) }); } return Timestamp{}; } @@ -518,8 +558,11 @@ inline Expected TreeNode::getInputStamped(const std::string& key, if(!config().blackboard) { - return nonstd::make_unexpected("getInput(): trying to access " - "an invalid Blackboard"); + // clang-format off + return nonstd::make_unexpected(PortInputError{ + PortError::InvalidBlackboard, + "getInput(): trying to access an invalid Blackboard" }); + // clang-format on } if(auto entry = config().blackboard->getEntry(std::string(blackboard_key))) @@ -549,9 +592,13 @@ inline Expected TreeNode::getInputStamped(const std::string& key, 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."); + return nonstd::make_unexpected(PortInputError{ PortError::CastFailed, + "Invalid cast requested " + "from vector to " + "vector." + " Element type does not " + "align." }); } destination = T(); std::transform( @@ -570,16 +617,41 @@ inline Expected TreeNode::getInputStamped(const std::string& key, } return Timestamp{ entry->sequence_id, entry->stamp }; } + + // The entry exists on the blackboard but its value is empty (for example, + // it was created via createEntry() without ever being set). + return nonstd::make_unexpected( + PortInputError{ PortError::BlackboardEntryEmpty, + StrCat("getInput() failed because it was unable to " + "find the key [", + key, "] remapped to [", blackboard_key, "]") }); } - return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to " - "find the key [", - key, "] remapped to [", blackboard_key, "]")); + // No entry on the blackboard at all: the port was wired to a key that was + // never populated. This is the case that callers most often want to surface + // as a warning, because it usually indicates a wiring mistake by the user. + return nonstd::make_unexpected( + PortInputError{ PortError::BlackboardKeyNotFound, + StrCat("getInput() failed because it was unable to " + "find the key [", + key, "] remapped to [", blackboard_key, "]") }); } catch(std::exception& err) { - return nonstd::make_unexpected(err.what()); + return nonstd::make_unexpected(PortInputError{ PortError::CastFailed, err.what() }); + } +} + +template +inline Expected TreeNode::getInputStamped(const std::string& key, + T& destination) const +{ + auto res = getInputStampedWithDiagnostic(key, destination); + if(res) + { + return res.value(); } + return nonstd::make_unexpected(std::move(res.error().message)); } template diff --git a/tests/gtest_ports.cpp b/tests/gtest_ports.cpp index abb2809fc..000ac71b2 100644 --- a/tests/gtest_ports.cpp +++ b/tests/gtest_ports.cpp @@ -871,3 +871,272 @@ TEST(PortTest, WhitespaceInPortName) ASSERT_ANY_THROW(BT::InputPort("trailing ")); ASSERT_NO_THROW(BT::InputPort("valid_port_name")); } + +// ---------------------------------------------------------------------------- +// Diagnostic-aware input port reads. +// +// getInputStampedWithDiagnostic() returns a structured PortInputError alongside +// the human-readable message so callers can tell unwired ports apart from +// blackboard wiring that was never populated (the latter is a likely user bug +// that should be surfaced). +// ---------------------------------------------------------------------------- + +class DiagnosticPortNode : public SyncActionNode +{ +public: + DiagnosticPortNode(const std::string& name, const NodeConfig& config) + : SyncActionNode(name, config) + {} + + NodeStatus tick() override + { + return NodeStatus::SUCCESS; + } + + static PortsList providedPorts() + { + // Two input ports: one with no default ("required" from the manifest's + // perspective) and one with a default of 7. + return { BT::InputPort("no_default"), + BT::InputPort("with_default", 7, "default seven") }; + } +}; + +TEST(PortTest, DiagnosticBlackboardKeyNotFound) +{ + // Port is wired to {missing_key}, but the blackboard has no such entry. + // This is the bug case the diagnostic enum exists to surface. + auto bb = Blackboard::create(); + NodeConfig config; + config.blackboard = bb; + config.input_ports["no_default"] = "{missing_key}"; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("no_default", value); + ASSERT_FALSE(res.has_value()); + EXPECT_EQ(res.error().code, PortError::BlackboardKeyNotFound); + EXPECT_FALSE(res.error().message.empty()); +} + +TEST(PortTest, DiagnosticBlackboardEntryEmpty) +{ + // The blackboard key exists, but its entry is empty (created without a value). + auto bb = Blackboard::create(); + bb->createEntry("empty_key", TypeInfo::Create()); + + NodeConfig config; + config.blackboard = bb; + config.input_ports["no_default"] = "{empty_key}"; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("no_default", value); + ASSERT_FALSE(res.has_value()); + EXPECT_EQ(res.error().code, PortError::BlackboardEntryEmpty); + EXPECT_FALSE(res.error().message.empty()); +} + +TEST(PortTest, DiagnosticManifestKeyMissing) +{ + // Ask for a key that is not in the XML and not in the manifest. + auto bb = Blackboard::create(); + NodeConfig config; + config.blackboard = bb; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("totally_unknown", value); + ASSERT_FALSE(res.has_value()); + EXPECT_EQ(res.error().code, PortError::ManifestKeyMissing); + EXPECT_FALSE(res.error().message.empty()); +} + +TEST(PortTest, DiagnosticNoDefaultNoWiring) +{ + // The manifest has the key but the default is empty AND nothing wired it. + auto bb = Blackboard::create(); + NodeConfig config; + config.blackboard = bb; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("no_default", value); + ASSERT_FALSE(res.has_value()); + EXPECT_EQ(res.error().code, PortError::NoDefaultNoWiring); + EXPECT_FALSE(res.error().message.empty()); +} + +TEST(PortTest, DiagnosticManifestMissing) +{ + // The node has no manifest attached and no input_ports entry for the key. + auto bb = Blackboard::create(); + NodeConfig config; + config.blackboard = bb; + config.manifest = nullptr; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("no_default", value); + ASSERT_FALSE(res.has_value()); + EXPECT_EQ(res.error().code, PortError::ManifestMissing); + EXPECT_FALSE(res.error().message.empty()); +} + +TEST(PortTest, DiagnosticConversionFailed) +{ + // A literal (non-blackboard) value that cannot be parsed as int. + auto bb = Blackboard::create(); + NodeConfig config; + config.blackboard = bb; + config.input_ports["no_default"] = "not_an_int"; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("no_default", value); + ASSERT_FALSE(res.has_value()); + EXPECT_EQ(res.error().code, PortError::ConversionFailed); + EXPECT_FALSE(res.error().message.empty()); +} + +TEST(PortTest, DiagnosticInvalidBlackboard) +{ + // Port is wired to {foo} but no blackboard is attached. + NodeConfig config; + config.blackboard = nullptr; + config.input_ports["no_default"] = "{foo}"; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("no_default", value); + ASSERT_FALSE(res.has_value()); + EXPECT_EQ(res.error().code, PortError::InvalidBlackboard); + EXPECT_FALSE(res.error().message.empty()); +} + +TEST(PortTest, DiagnosticSuccessFromBlackboard) +{ + // Happy path: blackboard has the entry, returns the value, no error. + auto bb = Blackboard::create(); + bb->set("my_value", 42); + + NodeConfig config; + config.blackboard = bb; + config.input_ports["no_default"] = "{my_value}"; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("no_default", value); + ASSERT_TRUE(res.has_value()) << (res ? "" : res.error().message); + EXPECT_EQ(value, 42); +} + +TEST(PortTest, DiagnosticSuccessFromDefault) +{ + // Happy path: nothing wired, manifest default fires. + auto bb = Blackboard::create(); + NodeConfig config; + config.blackboard = bb; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStampedWithDiagnostic("with_default", value); + ASSERT_TRUE(res.has_value()) << (res ? "" : res.error().message); + EXPECT_EQ(value, 7); +} + +TEST(PortTest, GetInputStampedDelegatesToDiagnosticMessage) +{ + // The original getInputStamped() must keep its existing message-on-failure + // contract so callers that relied on the string still work. + auto bb = Blackboard::create(); + NodeConfig config; + config.blackboard = bb; + config.input_ports["no_default"] = "{missing_key}"; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + int value = 0; + auto res = node.getInputStamped("no_default", value); + ASSERT_FALSE(res.has_value()); + EXPECT_FALSE(res.error().empty()); +} + +TEST(PortTest, GetInputWithDiagnosticReturnsValueOnSuccess) +{ + // Happy path: getInputWithDiagnostic(key) should return the port value + // directly, without the caller having to stage a destination variable. + auto bb = Blackboard::create(); + bb->set("my_value", 42); + + NodeConfig config; + config.blackboard = bb; + config.input_ports["no_default"] = "{my_value}"; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + auto res = node.getInputWithDiagnostic("no_default"); + ASSERT_TRUE(res.has_value()) << (res ? "" : res.error().message); + EXPECT_EQ(*res, 42); +} + +TEST(PortTest, GetInputWithDiagnosticPropagatesBlackboardKeyNotFound) +{ + // Failure path: getInputWithDiagnostic(key) should surface the same + // structured PortInputError the stamped variant produces. This is the + // error code MoveIt Pro's getOptionalInputs() relies on to distinguish a + // wiring bug from a legitimate "no value" optional port. + auto bb = Blackboard::create(); + NodeConfig config; + config.blackboard = bb; + config.input_ports["no_default"] = "{missing_key}"; + TreeNodeManifest manifest; + manifest.ports = DiagnosticPortNode::providedPorts(); + config.manifest = &manifest; + + DiagnosticPortNode node("diag", config); + + auto res = node.getInputWithDiagnostic("no_default"); + ASSERT_FALSE(res.has_value()); + EXPECT_EQ(res.error().code, PortError::BlackboardKeyNotFound); + EXPECT_FALSE(res.error().message.empty()); +}