From cb5e0649c4712500138d439c0f5c5ec4f6a627c0 Mon Sep 17 00:00:00 2001 From: Will Yingling Date: Mon, 20 Apr 2026 11:38:19 -0600 Subject: [PATCH 1/5] feat(blackboard): add GetBlackboardByKey behavior for dynamic-key reads --- CMakeLists.txt | 1 + .../get_blackboard_by_key.hpp | 43 +++++++++ src/get_blackboard_by_key.cpp | 96 +++++++++++++++++++ src/register_behaviors.cpp | 2 + test/test_behavior_plugins.cpp | 2 + 5 files changed, 144 insertions(+) create mode 100644 include/experimental_behaviors/get_blackboard_by_key.hpp create mode 100644 src/get_blackboard_by_key.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b12b6c2..17f56f2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ add_library( src/access_interface_value_from_group.cpp src/get_pose_stamped_from_topic.cpp src/publish_dynamic_interface_group_values.cpp + src/get_blackboard_by_key.cpp src/register_behaviors.cpp) target_include_directories( experimental_behaviors diff --git a/include/experimental_behaviors/get_blackboard_by_key.hpp b/include/experimental_behaviors/get_blackboard_by_key.hpp new file mode 100644 index 0000000..f55a8d1 --- /dev/null +++ b/include/experimental_behaviors/get_blackboard_by_key.hpp @@ -0,0 +1,43 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#pragma once + +#include +#include +#include + +namespace experimental_behaviors +{ +/** + * @brief Reads a blackboard entry whose key is determined dynamically at tick + * time from an input port, and copies its value to an output port. + * + * @details + * This is the read-side complement of BT.CPP's native SetBlackboard, which + * already supports a dynamic `output_key` port but has no corresponding + * read-by-dynamic-key primitive. + * + * | Data Port Name | Port Type | Object Type | + * | -------------- | --------- | ------------- | + * | key | input | std::string | + * | value | output | any (BT::Any) | + * + * The `key` port accepts both literal strings and root-scope references + * prefixed with `@`. Returns FAILURE if the key is missing or empty. + */ +class GetBlackboardByKey final : public moveit_pro::behaviors::SharedResourcesNode +{ +public: + GetBlackboardByKey(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + static BT::PortsList providedPorts(); + static BT::KeyValueVector metadata(); + + BT::NodeStatus tick() override; +}; +} // namespace experimental_behaviors diff --git a/src/get_blackboard_by_key.cpp b/src/get_blackboard_by_key.cpp new file mode 100644 index 0000000..92d8863 --- /dev/null +++ b/src/get_blackboard_by_key.cpp @@ -0,0 +1,96 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#include + +#include + +namespace +{ +inline constexpr auto kDescriptionGetBlackboardByKey = R"( +

+ Reads a blackboard entry whose key is computed at runtime (typically via a Script node + building a string from other blackboard variables) and copies the entry's value to an + output port. The key may be prefixed with '@' to address the root blackboard. +

+

+ Returns FAILURE if the key refers to a missing blackboard entry. This is the read-side + complement to BT.CPP's native SetBlackboard. +

+ )"; + +constexpr auto kPortIDKey = "key"; +constexpr auto kPortIDValue = "value"; +} // namespace + +namespace experimental_behaviors +{ +GetBlackboardByKey::GetBlackboardByKey( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList GetBlackboardByKey::providedPorts() +{ + return { BT::InputPort(kPortIDKey, + "Blackboard key to read from. May be constructed dynamically via Script. " + "Prefix with '@' for root-scope access."), + BT::OutputPort(kPortIDValue, "Value read from the blackboard entry referenced by `key`.") }; +} + +BT::KeyValueVector GetBlackboardByKey::metadata() +{ + return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "Blackboard" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, kDescriptionGetBlackboardByKey } }; +} + +BT::NodeStatus GetBlackboardByKey::tick() +{ + std::string key; + if (!getInput(kPortIDKey, key) || key.empty()) + { + shared_resources_->logger->publishFailureMessage(name(), "Missing or empty input port [key]"); + return BT::NodeStatus::FAILURE; + } + + auto src_entry = config().blackboard->getEntry(key); + if (!src_entry) + { + shared_resources_->logger->publishFailureMessage( + name(), "Blackboard entry '" + key + "' does not exist (cache miss)."); + return BT::NodeStatus::FAILURE; + } + + if (src_entry->value.empty()) + { + shared_resources_->logger->publishFailureMessage( + name(), "Blackboard entry '" + key + "' exists but holds no value."); + return BT::NodeStatus::FAILURE; + } + + // Route through the blackboard directly so the stored BT::Any is copied + // without forcing a concrete type — the consumer's input port decides the + // type at read time. Mirrors the pattern of BT.CPP's SetBlackboardNode + // (see include/behaviortree_cpp/actions/set_blackboard_node.h) in reverse. + const auto output_remap = config().output_ports.find(kPortIDValue); + if (output_remap == config().output_ports.end()) + { + shared_resources_->logger->publishFailureMessage(name(), "Output port [value] is not connected."); + return BT::NodeStatus::FAILURE; + } + std::string dst_key = output_remap->second; + // Strip leading/trailing braces from blackboard pointer syntax "{name}". + if (dst_key.size() >= 2 && dst_key.front() == '{' && dst_key.back() == '}') + { + dst_key = dst_key.substr(1, dst_key.size() - 2); + } + config().blackboard->set(dst_key, src_entry->value); + + return BT::NodeStatus::SUCCESS; +} +} // namespace experimental_behaviors diff --git a/src/register_behaviors.cpp b/src/register_behaviors.cpp index 2549803..778d3bc 100644 --- a/src/register_behaviors.cpp +++ b/src/register_behaviors.cpp @@ -11,6 +11,7 @@ #include "experimental_behaviors/access_interface_value_from_group.hpp" #include "experimental_behaviors/create_dynamic_interface_group_values.hpp" #include "experimental_behaviors/create_interface_value.hpp" +#include "experimental_behaviors/get_blackboard_by_key.hpp" #include "experimental_behaviors/get_dynamic_interface_group_values.hpp" #include "experimental_behaviors/get_interface_value_from_group.hpp" #include "experimental_behaviors/get_pose_stamped_from_topic.hpp" @@ -40,6 +41,7 @@ class ExperimentalBehaviorsLoader : public moveit_pro::behaviors::SharedResource shared_resources); moveit_pro::behaviors::registerBehavior(factory, "AccessInterfaceValue", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "GetBlackboardByKey", shared_resources); } }; } // namespace experimental_behaviors diff --git a/test/test_behavior_plugins.cpp b/test/test_behavior_plugins.cpp index bba02e1..21eef65 100644 --- a/test/test_behavior_plugins.cpp +++ b/test/test_behavior_plugins.cpp @@ -26,6 +26,8 @@ TEST(BehaviorTests, test_load_behavior_plugins) // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_get_pose_stamped_from_topic", "GetPoseStampedFromTopic", BT::NodeConfiguration())); + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_get_blackboard_by_key", "GetBlackboardByKey", BT::NodeConfiguration())); } int main(int argc, char** argv) From 1234904bf4f20ceb272d4f553f3610e6587bac58 Mon Sep 17 00:00:00 2001 From: Will Yingling Date: Mon, 20 Apr 2026 12:47:13 -0600 Subject: [PATCH 2/5] fix(blackboard): use canonical setOutput and clarify docs - Replace hand-rolled brace-stripping with setOutput to handle SubTree auto-remap sentinels and whitespace correctly. - Document that empty-valued entries are reported as FAILURE. - Correct port type in docstring (AnyTypeAllowed, not BT::Any). --- .../get_blackboard_by_key.hpp | 13 +++++--- src/get_blackboard_by_key.cpp | 33 +++++++------------ 2 files changed, 21 insertions(+), 25 deletions(-) diff --git a/include/experimental_behaviors/get_blackboard_by_key.hpp b/include/experimental_behaviors/get_blackboard_by_key.hpp index f55a8d1..b0a0112 100644 --- a/include/experimental_behaviors/get_blackboard_by_key.hpp +++ b/include/experimental_behaviors/get_blackboard_by_key.hpp @@ -21,13 +21,18 @@ namespace experimental_behaviors * already supports a dynamic `output_key` port but has no corresponding * read-by-dynamic-key primitive. * - * | Data Port Name | Port Type | Object Type | - * | -------------- | --------- | ------------- | - * | key | input | std::string | - * | value | output | any (BT::Any) | + * | Data Port Name | Port Type | Object Type | + * | -------------- | --------- | -------------------- | + * | key | input | std::string | + * | value | output | any (AnyTypeAllowed) | * * The `key` port accepts both literal strings and root-scope references * prefixed with `@`. Returns FAILURE if the key is missing or empty. + * + * An entry that exists but holds an empty value is also treated as a cache + * miss and reported as FAILURE. Callers that need to distinguish "declared + * but uninitialized" from "never declared" should populate the entry with + * an explicit sentinel before reading. */ class GetBlackboardByKey final : public moveit_pro::behaviors::SharedResourcesNode { diff --git a/src/get_blackboard_by_key.cpp b/src/get_blackboard_by_key.cpp index 92d8863..5ade882 100644 --- a/src/get_blackboard_by_key.cpp +++ b/src/get_blackboard_by_key.cpp @@ -28,9 +28,8 @@ constexpr auto kPortIDValue = "value"; namespace experimental_behaviors { -GetBlackboardByKey::GetBlackboardByKey( - const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) +GetBlackboardByKey::GetBlackboardByKey(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) { } @@ -61,35 +60,27 @@ BT::NodeStatus GetBlackboardByKey::tick() auto src_entry = config().blackboard->getEntry(key); if (!src_entry) { - shared_resources_->logger->publishFailureMessage( - name(), "Blackboard entry '" + key + "' does not exist (cache miss)."); + shared_resources_->logger->publishFailureMessage(name(), + "Blackboard entry '" + key + "' does not exist (cache miss)."); return BT::NodeStatus::FAILURE; } if (src_entry->value.empty()) { - shared_resources_->logger->publishFailureMessage( - name(), "Blackboard entry '" + key + "' exists but holds no value."); + shared_resources_->logger->publishFailureMessage(name(), + "Blackboard entry '" + key + "' exists but holds no value."); return BT::NodeStatus::FAILURE; } - // Route through the blackboard directly so the stored BT::Any is copied - // without forcing a concrete type — the consumer's input port decides the - // type at read time. Mirrors the pattern of BT.CPP's SetBlackboardNode - // (see include/behaviortree_cpp/actions/set_blackboard_node.h) in reverse. - const auto output_remap = config().output_ports.find(kPortIDValue); - if (output_remap == config().output_ports.end()) + // Pass the stored BT::Any through the canonical setOutput path so SubTree + // auto-remapping and blackboard-pointer validation are handled consistently + // with the rest of BT.CPP (see tree_node.h setOutput). + const auto result = setOutput(kPortIDValue, src_entry->value); + if (!result) { - shared_resources_->logger->publishFailureMessage(name(), "Output port [value] is not connected."); + shared_resources_->logger->publishFailureMessage(name(), result.error()); return BT::NodeStatus::FAILURE; } - std::string dst_key = output_remap->second; - // Strip leading/trailing braces from blackboard pointer syntax "{name}". - if (dst_key.size() >= 2 && dst_key.front() == '{' && dst_key.back() == '}') - { - dst_key = dst_key.substr(1, dst_key.size() - 2); - } - config().blackboard->set(dst_key, src_entry->value); return BT::NodeStatus::SUCCESS; } From 23278c3208e680d953eec711b718756a21f616d1 Mon Sep 17 00:00:00 2001 From: Will Yingling Date: Mon, 20 Apr 2026 15:16:22 -0600 Subject: [PATCH 3/5] feat(blackboard): add SetBlackboardByKey for dynamic-key writes Write-side complement to GetBlackboardByKey. BT.CPP's native SetBlackboard passes string-typed sources through TypeInfo::parseString when the destination is not std::string; for AnyTypeAllowed destinations (entries created implicitly by Script `:=` on subtree ports) no converter is registered, so parseString returns an empty Any and silently clobbers the write. SetBlackboardByKey copies the source Any directly, preserving both type and value. Co-Authored-By: Claude Opus 4.7 (1M context) --- CMakeLists.txt | 1 + .../set_blackboard_by_key.hpp | 55 ++++++++ src/register_behaviors.cpp | 2 + src/set_blackboard_by_key.cpp | 122 ++++++++++++++++++ 4 files changed, 180 insertions(+) create mode 100644 include/experimental_behaviors/set_blackboard_by_key.hpp create mode 100644 src/set_blackboard_by_key.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 17f56f2..409e719 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,7 @@ add_library( src/get_pose_stamped_from_topic.cpp src/publish_dynamic_interface_group_values.cpp src/get_blackboard_by_key.cpp + src/set_blackboard_by_key.cpp src/register_behaviors.cpp) target_include_directories( experimental_behaviors diff --git a/include/experimental_behaviors/set_blackboard_by_key.hpp b/include/experimental_behaviors/set_blackboard_by_key.hpp new file mode 100644 index 0000000..d726326 --- /dev/null +++ b/include/experimental_behaviors/set_blackboard_by_key.hpp @@ -0,0 +1,55 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#pragma once + +#include +#include +#include + +namespace experimental_behaviors +{ +/** + * @brief Writes the value of an input port to a blackboard entry whose key is + * determined dynamically at tick time. + * + * @details + * Write-side complement to GetBlackboardByKey and a drop-in replacement for + * BT.CPP's native SetBlackboard when the source value is carried on a port + * typed as `AnyTypeAllowed` (e.g. a value produced by a Script `:=` + * assignment). + * + * BT.CPP's SetBlackboard runs a string-conversion step whenever the + * destination entry is not strictly `std::string` and the source value is a + * string. For `AnyTypeAllowed` destinations there is no registered converter, + * so the conversion produces an empty Any and silently clobbers the + * destination — see set_blackboard_node.h and basic_types.cpp::parseString. + * + * This behavior copies the source Any directly, preserving both type and + * value, which matches the semantics callers expect when caching arbitrary + * subtree outputs under runtime-computed keys. + * + * | Data Port Name | Port Type | Object Type | + * | -------------- | --------- | -------------------- | + * | key | input | std::string | + * | value | input | any (AnyTypeAllowed) | + * + * The `key` port accepts both literal strings and root-scope references + * prefixed with `@`. Returns FAILURE if `key` is missing/empty or if the + * `value` port refers to a blackboard entry that does not exist. + */ +class SetBlackboardByKey final : public moveit_pro::behaviors::SharedResourcesNode +{ +public: + SetBlackboardByKey(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + static BT::PortsList providedPorts(); + static BT::KeyValueVector metadata(); + + BT::NodeStatus tick() override; +}; +} // namespace experimental_behaviors diff --git a/src/register_behaviors.cpp b/src/register_behaviors.cpp index 778d3bc..cbe0b2b 100644 --- a/src/register_behaviors.cpp +++ b/src/register_behaviors.cpp @@ -16,6 +16,7 @@ #include "experimental_behaviors/get_interface_value_from_group.hpp" #include "experimental_behaviors/get_pose_stamped_from_topic.hpp" #include "experimental_behaviors/publish_dynamic_interface_group_values.hpp" +#include "experimental_behaviors/set_blackboard_by_key.hpp" #include @@ -42,6 +43,7 @@ class ExperimentalBehaviorsLoader : public moveit_pro::behaviors::SharedResource moveit_pro::behaviors::registerBehavior(factory, "AccessInterfaceValue", shared_resources); moveit_pro::behaviors::registerBehavior(factory, "GetBlackboardByKey", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "SetBlackboardByKey", shared_resources); } }; } // namespace experimental_behaviors diff --git a/src/set_blackboard_by_key.cpp b/src/set_blackboard_by_key.cpp new file mode 100644 index 0000000..ede87c1 --- /dev/null +++ b/src/set_blackboard_by_key.cpp @@ -0,0 +1,122 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#include + +#include + +namespace +{ +inline constexpr auto kDescriptionSetBlackboardByKey = R"( +

+ Writes a source port's value to a blackboard entry whose key is computed at runtime + (typically via a Script node building a string from other blackboard variables). + The key may be prefixed with '@' to address the root blackboard. +

+

+ Use this instead of BT.CPP's native SetBlackboard when the source is an Any-typed + port (e.g. produced by a Script `:=` assignment): SetBlackboard runs a + string-to-destination-type conversion that silently empties the value when the + destination type is AnyTypeAllowed and no converter is registered. This behavior + copies the source Any directly. +

+ )"; + +constexpr auto kPortIDKey = "key"; +constexpr auto kPortIDValue = "value"; +} // namespace + +namespace experimental_behaviors +{ +SetBlackboardByKey::SetBlackboardByKey(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList SetBlackboardByKey::providedPorts() +{ + return { BT::InputPort(kPortIDKey, + "Blackboard key to write to. May be constructed dynamically via Script. " + "Prefix with '@' for root-scope access."), + BT::InputPort(kPortIDValue, "Source value. Accepts a blackboard pointer ({some_port}) whose " + "Any contents are copied verbatim, or a literal string.") }; +} + +BT::KeyValueVector SetBlackboardByKey::metadata() +{ + return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "Blackboard" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, kDescriptionSetBlackboardByKey } }; +} + +BT::NodeStatus SetBlackboardByKey::tick() +{ + std::string key; + if (!getInput(kPortIDKey, key) || key.empty()) + { + shared_resources_->logger->publishFailureMessage(name(), "Missing or empty input port [key]"); + return BT::NodeStatus::FAILURE; + } + + // Read the raw port expression from config rather than going through + // getInput, which would stringify whatever the source port + // holds. We need the untouched Any so trajectories, vectors, etc. survive + // the copy. + const auto value_it = config().input_ports.find(kPortIDValue); + if (value_it == config().input_ports.end()) + { + shared_resources_->logger->publishFailureMessage(name(), "Missing input port [value]"); + return BT::NodeStatus::FAILURE; + } + const std::string& value_expr = value_it->second; + + BT::StringView stripped_key; + BT::Any out_value; + BT::TypeInfo src_info; + if (BT::TreeNode::isBlackboardPointer(value_expr, &stripped_key)) + { + const auto input_key = std::string(stripped_key); + auto src_entry = config().blackboard->getEntry(input_key); + if (!src_entry) + { + shared_resources_->logger->publishFailureMessage( + name(), "Source port '" + input_key + "' for [value] does not reference an existing blackboard entry."); + return BT::NodeStatus::FAILURE; + } + out_value = src_entry->value; + src_info = src_entry->info; + } + else + { + out_value = BT::Any(value_expr); + src_info = BT::TypeInfo::Create(); + } + + // Ensure the destination entry exists before writing. Create it with the + // source entry's TypeInfo so downstream consumers that inspect entry->info + // see a consistent type (e.g. AnyTypeAllowed vs std::string vs trajectory). + auto dst_entry = config().blackboard->getEntry(key); + if (!dst_entry) + { + config().blackboard->createEntry(key, src_info); + dst_entry = config().blackboard->getEntry(key); + if (!dst_entry) + { + shared_resources_->logger->publishFailureMessage(name(), "Failed to create blackboard entry '" + key + "'"); + return BT::NodeStatus::FAILURE; + } + } + + { + std::scoped_lock lock(dst_entry->entry_mutex); + dst_entry->value = out_value; + dst_entry->sequence_id++; + dst_entry->stamp = std::chrono::steady_clock::now().time_since_epoch(); + } + + return BT::NodeStatus::SUCCESS; +} +} // namespace experimental_behaviors From 6101af14064db3c711c7678b472d7f618e241531 Mon Sep 17 00:00:00 2001 From: Will Yingling Date: Tue, 21 Apr 2026 10:00:55 -0600 Subject: [PATCH 4/5] ci(workflow): bump reusable workflow to v0.0.7 v0.0.6 sources /opt/underlay_ws/install/setup.sh, which no longer exists in the picknikciuser/moveit-studio:main-humble image, so the Install rosdeps step fails before build or test run. v0.0.7 is the upstream fix that removes those three sourcing lines. Co-Authored-By: Claude Opus 4.7 (1M context) --- .github/workflows/CI.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/CI.yaml b/.github/workflows/CI.yaml index c2dd4f5..b429eb4 100644 --- a/.github/workflows/CI.yaml +++ b/.github/workflows/CI.yaml @@ -17,7 +17,7 @@ on: jobs: integration-test-in-studio-container: - uses: PickNikRobotics/moveit_pro_ci/.github/workflows/workspace_integration_test.yaml@v0.0.6 + uses: PickNikRobotics/moveit_pro_ci/.github/workflows/workspace_integration_test.yaml@v0.0.7 with: runner: "ubuntu-22.04" image_tag: ${{ github.event_name == 'pull_request' && github.event.pull_request.base.ref || github.ref_name }} From aeaea76f907c33f47426ba51b96a653b58b17535 Mon Sep 17 00:00:00 2001 From: Will Yingling Date: Thu, 30 Apr 2026 12:30:28 -0600 Subject: [PATCH 5/5] docs(blackboard): enumerate all FAILURE conditions in GetBlackboardByKey description Description previously only mentioned the missing-entry case, but tick() also returns FAILURE for a missing/empty `key` port and for entries that exist but hold no value. Update the description string so the UI help text matches the actual semantics. Co-Authored-By: Claude Opus 4.7 (1M context) --- src/get_blackboard_by_key.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/get_blackboard_by_key.cpp b/src/get_blackboard_by_key.cpp index 5ade882..d56c9c7 100644 --- a/src/get_blackboard_by_key.cpp +++ b/src/get_blackboard_by_key.cpp @@ -17,8 +17,9 @@ inline constexpr auto kDescriptionGetBlackboardByKey = R"( output port. The key may be prefixed with '@' to address the root blackboard.

- Returns FAILURE if the key refers to a missing blackboard entry. This is the read-side - complement to BT.CPP's native SetBlackboard. + Returns FAILURE if the input key port is missing or empty, if the key + refers to a blackboard entry that does not exist, or if the referenced entry exists + but holds no value. This is the read-side complement to BT.CPP's native SetBlackboard.

)";