From 6eafff4ee4beb6db614aeedfafece1124381bac0 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 14:48:30 +0800 Subject: [PATCH 1/6] Introduce a common TaskRemapper with wildcard support Signed-off-by: Luca Della Vedova --- nexus_common/CMakeLists.txt | 2 ++ nexus_system_orchestrator/src/context.hpp | 4 +-- .../src/execute_task.cpp | 8 +++--- .../src/system_orchestrator.cpp | 17 +++--------- .../src/system_orchestrator.hpp | 5 ++-- .../src/workcell_orchestrator.cpp | 26 +++++++++---------- .../src/workcell_orchestrator.hpp | 5 ++-- 7 files changed, 30 insertions(+), 37 deletions(-) diff --git a/nexus_common/CMakeLists.txt b/nexus_common/CMakeLists.txt index ef46734..1a20f9a 100644 --- a/nexus_common/CMakeLists.txt +++ b/nexus_common/CMakeLists.txt @@ -24,6 +24,7 @@ add_library(${PROJECT_NAME} SHARED src/node_thread.cpp src/node_utils.cpp src/pausable_sequence.cpp + src/task_remapper.cpp ) GENERATE_EXPORT_HEADER(${PROJECT_NAME} EXPORT_FILE_NAME "include/nexus_common_export.hpp" @@ -130,6 +131,7 @@ if(BUILD_TESTING) ) nexus_add_test(logging_test src/logging_test.cpp) nexus_add_test(sync_ros_client_test src/sync_ros_client_test.cpp) + nexus_add_test(task_remapper_test src/task_remapper_test.cpp) endif() ament_export_targets(${PROJECT_NAME}) diff --git a/nexus_system_orchestrator/src/context.hpp b/nexus_system_orchestrator/src/context.hpp index d50470e..3f0f719 100644 --- a/nexus_system_orchestrator/src/context.hpp +++ b/nexus_system_orchestrator/src/context.hpp @@ -20,6 +20,7 @@ #include "session.hpp" +#include #include #include #include @@ -43,8 +44,7 @@ public: rclcpp_lifecycle::LifecycleNode& node; public: std::string job_id; public: WorkOrder wo; public: std::vector tasks; -public: std::shared_ptr> task_remaps; +public: std::shared_ptr task_remapper; /** * Map of task ids and their assigned workcell ids. */ diff --git a/nexus_system_orchestrator/src/execute_task.cpp b/nexus_system_orchestrator/src/execute_task.cpp index fbf626a..b20330f 100644 --- a/nexus_system_orchestrator/src/execute_task.cpp +++ b/nexus_system_orchestrator/src/execute_task.cpp @@ -41,16 +41,16 @@ BT::NodeStatus ExecuteTask::onStart() // Remap the BT filename to load if one is provided. std::string bt_name = task->type; - auto it = _ctx->task_remaps->find(task->type); - if (it != _ctx->task_remaps->end()) + const auto new_task = _ctx->task_remapper->remap(task->type); + if (new_task != task->type) { RCLCPP_DEBUG( _ctx->node.get_logger(), "[ExecuteTask] Loading remapped BT [%s] for original task type [%s]", - it->second.c_str(), + new_task.c_str(), task->type.c_str() ); - bt_name = it->second; + bt_name = new_task; } std::filesystem::path task_bt_path(this->_bt_path / (bt_name + ".xml")); if (!std::filesystem::is_regular_file(task_bt_path)) diff --git a/nexus_system_orchestrator/src/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp index 9fbbaa8..441827c 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.cpp +++ b/nexus_system_orchestrator/src/system_orchestrator.cpp @@ -100,23 +100,12 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options) } { - _task_remaps = - std::make_shared>(); ParameterDescriptor desc; desc.read_only = true; desc.description = "A yaml containing a dictionary of task types and an array of remaps."; - const auto yaml = this->declare_parameter("remap_task_types", "", desc); - const auto remaps = YAML::Load(yaml); - for (const auto& n : remaps) - { - const auto task_type = n.first.as(); - const auto& mappings = n.second; - for (const auto& m : mappings) - { - this->_task_remaps->emplace(m.as(), task_type); - } - } + const auto param = this->declare_parameter("remap_task_types", "", desc); + this->_task_remapper = std::make_shared(param); } { @@ -545,7 +534,7 @@ void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal) // using `new` because make_shared does not work with aggregate initializer std::shared_ptr ctx{new Context{*this, - goal.order.id, wo, tasks, this->_task_remaps, + goal.order.id, wo, tasks, this->_task_remapper, std::unordered_map{}, this->_workcell_sessions, this->_transporter_sessions, {}, nullptr, diff --git a/nexus_system_orchestrator/src/system_orchestrator.hpp b/nexus_system_orchestrator/src/system_orchestrator.hpp index 51ac5b5..65fea01 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.hpp +++ b/nexus_system_orchestrator/src/system_orchestrator.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -96,8 +97,8 @@ class SystemOrchestrator : public std::unique_ptr> _lifecycle_mgr{nullptr}; rclcpp::TimerBase::SharedPtr _pre_configure_timer; rclcpp::SubscriptionBase::SharedPtr _estop_sub; - // mapping of mapped task type and the original - std::shared_ptr> _task_remaps; + // Manages task remapping + std::shared_ptr _task_remapper; std::shared_ptr _param_cb_handle; /** diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp index 7ab1438..5c66eaf 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp @@ -92,18 +92,8 @@ WorkcellOrchestrator::WorkcellOrchestrator(const rclcpp::NodeOptions& options) desc.read_only = true; desc.description = "A yaml containing a dictionary of task types and an array of remaps."; - const auto yaml = this->declare_parameter("remap_task_types", std::string{}, - desc); - const auto remaps = YAML::Load(yaml); - for (const auto& n : remaps) - { - const auto task_type = n.first.as(); - const auto& mappings = n.second; - for (const auto& m : mappings) - { - this->_task_remaps.emplace(m.as(), task_type); - } - } + const auto param = this->declare_parameter("remap_task_types", "", desc); + this->_task_remapper = std::make_shared(param); } { ParameterDescriptor desc; @@ -988,8 +978,18 @@ BT::Tree WorkcellOrchestrator::_create_bt(const std::shared_ptr& ctx) { // To keep things simple, the task type is used as the key for the behavior tree to use. this->_ctx_mgr->set_active_context(ctx); + const auto new_task = this->_task_remapper->remap(ctx->task.type); + if (new_task != ctx->task.type) + { + RCLCPP_DEBUG( + this->get_logger(), + "Loading remapped BT [%s] for original task type [%s]", + new_task.c_str(), + ctx->task.type.c_str() + ); + } return this->_bt_factory->createTreeFromFile(this->_bt_store.get_bt( - ctx->task.type)); + new_task)); } void WorkcellOrchestrator::_handle_command_success( diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp index 30813bc..268febe 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -124,8 +125,8 @@ private: std::list> _ctxs; private: std::shared_ptr _ctx_mgr; private: std::unique_ptr> _lifecycle_mgr{ nullptr}; -// mapping of mapped task type and the original -private: std::unordered_map _task_remaps; +// Takes care of remapping tasks +private: std::shared_ptr _task_remapper; private: TaskParser _task_parser; private: pluginlib::ClassLoader _task_checker_loader; private: std::shared_ptr _task_checker; From a8c5bf433f478ad18b5d908d42e661e55c4e59c3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 14:50:56 +0800 Subject: [PATCH 2/6] Add task remapper Signed-off-by: Luca Della Vedova --- .../include/nexus_common/task_remapper.hpp | 51 ++++++++++++++++ nexus_common/src/task_remapper.cpp | 61 +++++++++++++++++++ nexus_common/src/task_remapper_test.cpp | 49 +++++++++++++++ 3 files changed, 161 insertions(+) create mode 100644 nexus_common/include/nexus_common/task_remapper.hpp create mode 100644 nexus_common/src/task_remapper.cpp create mode 100644 nexus_common/src/task_remapper_test.cpp diff --git a/nexus_common/include/nexus_common/task_remapper.hpp b/nexus_common/include/nexus_common/task_remapper.hpp new file mode 100644 index 0000000..6eb5d39 --- /dev/null +++ b/nexus_common/include/nexus_common/task_remapper.hpp @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef NEXUS_COMMON__TASK_REMAPPER_HPP +#define NEXUS_COMMON__TASK_REMAPPER_HPP + +#include "nexus_common_export.hpp" + +#include + +namespace nexus::common { + +/** + * Provides task remapping capability + */ +class NEXUS_COMMON_EXPORT TaskRemapper +{ +public: + /* + * Initialize the remapper with the value of a ROS parameter containing a YAML + */ + TaskRemapper(const std::string& param); + + /* + * Remaps, if necessary, the input task + */ + std::string remap(const std::string& task) const; + +private: + // If present, match every incoming task to the target task + std::optional _wildcard_match; + std::unordered_map _task_remaps; +}; + +} + +#endif diff --git a/nexus_common/src/task_remapper.cpp b/nexus_common/src/task_remapper.cpp new file mode 100644 index 0000000..21da6c1 --- /dev/null +++ b/nexus_common/src/task_remapper.cpp @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "task_remapper.hpp" + +#include + +namespace nexus::common { + +TaskRemapper::TaskRemapper(const std::string& param) +{ + const auto remaps = YAML::Load(param); + for (const auto& n : remaps) + { + const auto task_type = n.first.as(); + const auto& mappings = n.second; + for (const auto& m : mappings) + { + auto mapping = m.as(); + if (mapping == "*") + { + this->_wildcard_match = task_type; + this->_task_remaps.clear(); + return; + } + // TODO(luca) check for duplicates, logging if found + this->_task_remaps.emplace(mapping, task_type); + } + } +} + +std::string TaskRemapper::remap(const std::string& task) const +{ + if (this->_wildcard_match.has_value()) + { + return this->_wildcard_match.value(); + } + const auto it = this->_task_remaps.find(task); + if (it != this->_task_remaps.end()) + { + return it->second; + } + return task; +} + + +} diff --git a/nexus_common/src/task_remapper_test.cpp b/nexus_common/src/task_remapper_test.cpp new file mode 100644 index 0000000..56215b6 --- /dev/null +++ b/nexus_common/src/task_remapper_test.cpp @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2023 Johnson & Johnson + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#define CATCH_CONFIG_MAIN +#include + +#include "task_remapper.hpp" +#include "test_utils.hpp" + +namespace nexus::common::test { + +TEST_CASE("task_remapping") { + std::string param = + R"( + pick_and_place: [pick, place] + )"; + auto remapper = TaskRemapper(param); + CHECK(remapper.remap("pick") == "pick_and_place"); + CHECK(remapper.remap("place") == "pick_and_place"); + CHECK(remapper.remap("other") == "other"); +} + +TEST_CASE("task_remapping_with_wildcard") { + std::string param = + R"( + pick_and_place: [pick, place] + main : ["*"] + )"; + auto remapper = TaskRemapper(param); + CHECK(remapper.remap("pick") == "main"); + CHECK(remapper.remap("place") == "main"); + CHECK(remapper.remap("other") == "main"); +} + +} From a7297a546b75a0d9a05aa1bbbd8e1196261d9c32 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 14:54:32 +0800 Subject: [PATCH 3/6] Minor cleanup in test code Signed-off-by: Luca Della Vedova --- nexus_common/src/task_remapper_test.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nexus_common/src/task_remapper_test.cpp b/nexus_common/src/task_remapper_test.cpp index 56215b6..c5b0061 100644 --- a/nexus_common/src/task_remapper_test.cpp +++ b/nexus_common/src/task_remapper_test.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2023 Johnson & Johnson + * Copyright (C) 2024 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -19,7 +19,6 @@ #include #include "task_remapper.hpp" -#include "test_utils.hpp" namespace nexus::common::test { From c61743e0d23a98debf40215accece5f82cbf2e28 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 16:31:28 +0800 Subject: [PATCH 4/6] Copyright Signed-off-by: Luca Della Vedova --- nexus_common/include/nexus_common/task_remapper.hpp | 2 +- nexus_common/src/task_remapper.cpp | 2 +- nexus_common/src/task_remapper_test.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nexus_common/include/nexus_common/task_remapper.hpp b/nexus_common/include/nexus_common/task_remapper.hpp index 6eb5d39..ab7e9dc 100644 --- a/nexus_common/include/nexus_common/task_remapper.hpp +++ b/nexus_common/include/nexus_common/task_remapper.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2024 Open Source Robotics Foundation + * Copyright (C) 2025 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/nexus_common/src/task_remapper.cpp b/nexus_common/src/task_remapper.cpp index 21da6c1..8bf0f28 100644 --- a/nexus_common/src/task_remapper.cpp +++ b/nexus_common/src/task_remapper.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2024 Open Source Robotics Foundation + * Copyright (C) 2025 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/nexus_common/src/task_remapper_test.cpp b/nexus_common/src/task_remapper_test.cpp index c5b0061..2c90453 100644 --- a/nexus_common/src/task_remapper_test.cpp +++ b/nexus_common/src/task_remapper_test.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2024 Open Source Robotics Foundation + * Copyright (C) 2025 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. From b881d1e2046602ce3f909ac9df26412bc5935e94 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 10 Jan 2025 17:34:35 +0800 Subject: [PATCH 5/6] Change API to std::optional Signed-off-by: Luca Della Vedova --- nexus_common/include/nexus_common/task_remapper.hpp | 6 +++++- nexus_common/src/task_remapper.cpp | 4 ++-- nexus_system_orchestrator/src/execute_task.cpp | 6 +++--- nexus_workcell_orchestrator/src/workcell_orchestrator.cpp | 8 +++++--- 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/nexus_common/include/nexus_common/task_remapper.hpp b/nexus_common/include/nexus_common/task_remapper.hpp index ab7e9dc..15a3833 100644 --- a/nexus_common/include/nexus_common/task_remapper.hpp +++ b/nexus_common/include/nexus_common/task_remapper.hpp @@ -22,6 +22,9 @@ #include +#include +#include + namespace nexus::common { /** @@ -37,8 +40,9 @@ class NEXUS_COMMON_EXPORT TaskRemapper /* * Remaps, if necessary, the input task + * Returns a value if the task was remapped, std::nullopt otherwise */ - std::string remap(const std::string& task) const; + std::optional remap(const std::string& task) const; private: // If present, match every incoming task to the target task diff --git a/nexus_common/src/task_remapper.cpp b/nexus_common/src/task_remapper.cpp index 8bf0f28..2e6b458 100644 --- a/nexus_common/src/task_remapper.cpp +++ b/nexus_common/src/task_remapper.cpp @@ -43,7 +43,7 @@ TaskRemapper::TaskRemapper(const std::string& param) } } -std::string TaskRemapper::remap(const std::string& task) const +std::optional TaskRemapper::remap(const std::string& task) const { if (this->_wildcard_match.has_value()) { @@ -54,7 +54,7 @@ std::string TaskRemapper::remap(const std::string& task) const { return it->second; } - return task; + return std::nullopt; } diff --git a/nexus_system_orchestrator/src/execute_task.cpp b/nexus_system_orchestrator/src/execute_task.cpp index b20330f..f3ce222 100644 --- a/nexus_system_orchestrator/src/execute_task.cpp +++ b/nexus_system_orchestrator/src/execute_task.cpp @@ -42,15 +42,15 @@ BT::NodeStatus ExecuteTask::onStart() // Remap the BT filename to load if one is provided. std::string bt_name = task->type; const auto new_task = _ctx->task_remapper->remap(task->type); - if (new_task != task->type) + if (new_task.has_value()) { RCLCPP_DEBUG( _ctx->node.get_logger(), "[ExecuteTask] Loading remapped BT [%s] for original task type [%s]", - new_task.c_str(), + new_task.value().c_str(), task->type.c_str() ); - bt_name = new_task; + bt_name = new_task.value(); } std::filesystem::path task_bt_path(this->_bt_path / (bt_name + ".xml")); if (!std::filesystem::is_regular_file(task_bt_path)) diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp index 5c66eaf..c404f13 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp @@ -979,17 +979,19 @@ BT::Tree WorkcellOrchestrator::_create_bt(const std::shared_ptr& ctx) // To keep things simple, the task type is used as the key for the behavior tree to use. this->_ctx_mgr->set_active_context(ctx); const auto new_task = this->_task_remapper->remap(ctx->task.type); - if (new_task != ctx->task.type) + auto bt_name = ctx->task.type; + if (new_task.has_value()) { RCLCPP_DEBUG( this->get_logger(), "Loading remapped BT [%s] for original task type [%s]", - new_task.c_str(), + new_task.value().c_str(), ctx->task.type.c_str() ); + bt_name = new_task.value(); } return this->_bt_factory->createTreeFromFile(this->_bt_store.get_bt( - new_task)); + bt_name)); } void WorkcellOrchestrator::_handle_command_success( From 8c62c69275eebd5361d44ed37f037567347a4598 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 10 Jan 2025 17:37:14 +0800 Subject: [PATCH 6/6] Add test case Signed-off-by: Luca Della Vedova --- nexus_common/src/task_remapper_test.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/nexus_common/src/task_remapper_test.cpp b/nexus_common/src/task_remapper_test.cpp index 2c90453..44c8bdf 100644 --- a/nexus_common/src/task_remapper_test.cpp +++ b/nexus_common/src/task_remapper_test.cpp @@ -30,7 +30,7 @@ TEST_CASE("task_remapping") { auto remapper = TaskRemapper(param); CHECK(remapper.remap("pick") == "pick_and_place"); CHECK(remapper.remap("place") == "pick_and_place"); - CHECK(remapper.remap("other") == "other"); + CHECK(remapper.remap("other") == std::nullopt); } TEST_CASE("task_remapping_with_wildcard") { @@ -45,4 +45,14 @@ TEST_CASE("task_remapping_with_wildcard") { CHECK(remapper.remap("other") == "main"); } +TEST_CASE("task_remapping_with_normal_and_wildcard") { + std::string param = + R"( + pick_and_place: [pick, "*"] + )"; + auto remapper = TaskRemapper(param); + CHECK(remapper.remap("pick") == "pick_and_place"); + CHECK(remapper.remap("place") == "pick_and_place"); +} + }