From 7c6ebf7851b90d0683583e745012809f8f5ae0d5 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 14 Jan 2025 03:20:10 +0800 Subject: [PATCH] Introduce a common TaskRemapper with wildcard support (#55) * Introduce a common TaskRemapper with wildcard support Signed-off-by: Luca Della Vedova * Add task remapper Signed-off-by: Luca Della Vedova * Minor cleanup in test code Signed-off-by: Luca Della Vedova * Copyright Signed-off-by: Luca Della Vedova * Change API to std::optional Signed-off-by: Luca Della Vedova * Add test case Signed-off-by: Luca Della Vedova * Change TaskRemapper API, remove duplicated implementation Signed-off-by: Luca Della Vedova * Create task_remapper inside on_configure for system_orchestrator Signed-off-by: Yadunund --------- Signed-off-by: Luca Della Vedova Signed-off-by: Yadunund Co-authored-by: Yadunund --- nexus_common/CMakeLists.txt | 2 + .../include/nexus_common/task_remapper.hpp | 68 +++++++++++++++++++ nexus_common/src/task_remapper.cpp | 58 ++++++++++++++++ nexus_common/src/task_remapper_test.cpp | 63 +++++++++++++++++ nexus_system_orchestrator/src/context.hpp | 4 +- .../src/execute_task.cpp | 8 +-- .../src/system_orchestrator.cpp | 31 +++++---- .../src/system_orchestrator.hpp | 5 +- .../src/task_parser.cpp | 21 +----- .../src/task_parser.hpp | 8 --- .../src/workcell_orchestrator.cpp | 36 ++++------ .../src/workcell_orchestrator.hpp | 5 +- 12 files changed, 236 insertions(+), 73 deletions(-) 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/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_common/include/nexus_common/task_remapper.hpp b/nexus_common/include/nexus_common/task_remapper.hpp new file mode 100644 index 0000000..69906f0 --- /dev/null +++ b/nexus_common/include/nexus_common/task_remapper.hpp @@ -0,0 +1,68 @@ +/* + * 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. + * 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 + +#include + +#include +#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. + * + * The YAML should have a sequence of key:value types where the key is the task to + * remap to, and the value is a list of tasks to remap to the key. + * For example: + * + * pick_and_place: [pick, place] + * a_and_b: [a, b] + * + * Will remap "pick" or "place" to "pick_and_place", "a" and "b" to "a_and_b". + * Note: If the value is specified as an asterisk, "*", any value will be + * remapped to the specified key. + */ + TaskRemapper(const YAML::Node& remaps); + + /* + * Remaps, if necessary, the input task + * Returns a value if the task was remapped, std::nullopt otherwise + */ + std::optional 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..b0bc792 --- /dev/null +++ b/nexus_common/src/task_remapper.cpp @@ -0,0 +1,58 @@ +/* + * 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. + * 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" + +namespace nexus::common { + +TaskRemapper::TaskRemapper(const YAML::Node& remaps) +{ + 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::optional 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 std::nullopt; +} + + +} diff --git a/nexus_common/src/task_remapper_test.cpp b/nexus_common/src/task_remapper_test.cpp new file mode 100644 index 0000000..874d8a6 --- /dev/null +++ b/nexus_common/src/task_remapper_test.cpp @@ -0,0 +1,63 @@ +/* + * 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. + * 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 + +#include "task_remapper.hpp" + +namespace nexus::common::test { + +TEST_CASE("task_remapping") { + std::string param = + R"( + pick_and_place: [pick, place] + )"; + const auto yaml = YAML::Load(param); + const auto remapper = TaskRemapper(yaml); + CHECK(remapper.remap("pick") == "pick_and_place"); + CHECK(remapper.remap("place") == "pick_and_place"); + CHECK(remapper.remap("other") == std::nullopt); +} + +TEST_CASE("task_remapping_with_wildcard") { + std::string param = + R"( + pick_and_place: [pick, place] + main : ["*"] + )"; + const auto yaml = YAML::Load(param); + const auto remapper = TaskRemapper(yaml); + CHECK(remapper.remap("pick") == "main"); + CHECK(remapper.remap("place") == "main"); + CHECK(remapper.remap("other") == "main"); +} + +TEST_CASE("task_remapping_with_normal_and_wildcard") { + std::string param = + R"( + pick_and_place: [pick, "*"] + )"; + const auto yaml = YAML::Load(param); + const auto remapper = TaskRemapper(yaml); + CHECK(remapper.remap("pick") == "pick_and_place"); + CHECK(remapper.remap("place") == "pick_and_place"); +} + +} 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..f3ce222 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.has_value()) { RCLCPP_DEBUG( _ctx->node.get_logger(), "[ExecuteTask] Loading remapped BT [%s] for original task type [%s]", - it->second.c_str(), + new_task.value().c_str(), task->type.c_str() ); - bt_name = it->second; + 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_system_orchestrator/src/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp index 9fbbaa8..5977a74 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.cpp +++ b/nexus_system_orchestrator/src/system_orchestrator.cpp @@ -100,23 +100,11 @@ 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); } { @@ -173,6 +161,21 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options) auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous) -> CallbackReturn { + // Create map for remapping capabilities. + const std::string remap_caps = this->get_parameter("remap_task_types").as_string(); + try + { + YAML::Node node = YAML::Load(remap_caps); + this->_task_remapper = std::make_shared(std::move(node)); + } + catch (YAML::ParserException& e) + { + RCLCPP_ERROR( + this->get_logger(), "Failed to parse remap_task_types parameter: (%s)", + e.what()); + return CallbackReturn::FAILURE; + } + // create list workcells service this->_list_workcells_srv = this->create_service( @@ -545,7 +548,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/task_parser.cpp b/nexus_workcell_orchestrator/src/task_parser.cpp index f3926b9..f45c1c3 100644 --- a/nexus_workcell_orchestrator/src/task_parser.cpp +++ b/nexus_workcell_orchestrator/src/task_parser.cpp @@ -29,29 +29,10 @@ Task TaskParser::parse_task( { return Task{ workcell_task.id, - this->remap_task_type(workcell_task.type), + workcell_task.type, YAML::Load(workcell_task.payload), YAML::Load(workcell_task.previous_results), }; } -//============================================================================== -void TaskParser::add_remap_task_type(const std::string& remap_from_type, - const std::string& remap_to_type) -{ - this->_remap_task_types[remap_from_type] = remap_to_type; -} - -//============================================================================== -std::string TaskParser::remap_task_type(const std::string& task_type) -{ - auto remap_it = this->_remap_task_types.find(task_type); - - if (remap_it != this->_remap_task_types.end()) - { - return remap_it->second; - } - return task_type; -} - } // namespace nexus::workcell_orchestrator diff --git a/nexus_workcell_orchestrator/src/task_parser.hpp b/nexus_workcell_orchestrator/src/task_parser.hpp index b93cf46..ed764a0 100644 --- a/nexus_workcell_orchestrator/src/task_parser.hpp +++ b/nexus_workcell_orchestrator/src/task_parser.hpp @@ -38,18 +38,10 @@ public: TaskParser() {} */ public: Task parse_task(const nexus_orchestrator_msgs::msg::WorkcellTask& task); - /** - * Add task type to remap to. - */ -public: void add_remap_task_type(const std::string& remap_from_type, - const std::string& remap_to_type); - /** * Remaps task types if a remap entry is found for the given type. */ public: std::string remap_task_type(const std::string& task_type); - -private: std::unordered_map _remap_task_types; }; } // namespace nexus::workcell_orchestrator diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp index 7ab1438..3e1b4e3 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp @@ -92,18 +92,7 @@ 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); } { ParameterDescriptor desc; @@ -695,14 +684,7 @@ auto WorkcellOrchestrator::_configure( try { YAML::Node node = YAML::Load(remap_caps); - for (YAML::const_iterator it = node.begin(); it != node.end(); ++it) - { - for (std::size_t i = 0; i < it->second.size(); ++i) - { - this->_task_parser.add_remap_task_type( - it->second[i].as(), it->first.as()); - } - } + this->_task_remapper = std::make_shared(node); } catch (YAML::ParserException& e) { @@ -988,8 +970,20 @@ 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); + 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.value().c_str(), + ctx->task.type.c_str() + ); + bt_name = new_task.value(); + } return this->_bt_factory->createTreeFromFile(this->_bt_store.get_bt( - ctx->task.type)); + bt_name)); } 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;