diff --git a/nexus_common/include/nexus_common/task_remapper.hpp b/nexus_common/include/nexus_common/task_remapper.hpp index 15a3833..3613266 100644 --- a/nexus_common/include/nexus_common/task_remapper.hpp +++ b/nexus_common/include/nexus_common/task_remapper.hpp @@ -22,6 +22,8 @@ #include +#include + #include #include @@ -34,9 +36,18 @@ class NEXUS_COMMON_EXPORT TaskRemapper { public: /* - * Initialize the remapper with the value of a ROS parameter containing a YAML + * 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" */ - TaskRemapper(const std::string& param); + TaskRemapper(const YAML::Node& remaps); /* * Remaps, if necessary, the input task diff --git a/nexus_common/src/task_remapper.cpp b/nexus_common/src/task_remapper.cpp index 2e6b458..b0bc792 100644 --- a/nexus_common/src/task_remapper.cpp +++ b/nexus_common/src/task_remapper.cpp @@ -17,13 +17,10 @@ #include "task_remapper.hpp" -#include - namespace nexus::common { -TaskRemapper::TaskRemapper(const std::string& param) +TaskRemapper::TaskRemapper(const YAML::Node& remaps) { - const auto remaps = YAML::Load(param); for (const auto& n : remaps) { const auto task_type = n.first.as(); diff --git a/nexus_common/src/task_remapper_test.cpp b/nexus_common/src/task_remapper_test.cpp index 44c8bdf..874d8a6 100644 --- a/nexus_common/src/task_remapper_test.cpp +++ b/nexus_common/src/task_remapper_test.cpp @@ -18,6 +18,8 @@ #define CATCH_CONFIG_MAIN #include +#include + #include "task_remapper.hpp" namespace nexus::common::test { @@ -27,7 +29,8 @@ TEST_CASE("task_remapping") { R"( pick_and_place: [pick, place] )"; - auto remapper = TaskRemapper(param); + 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); @@ -39,7 +42,8 @@ TEST_CASE("task_remapping_with_wildcard") { pick_and_place: [pick, place] main : ["*"] )"; - auto remapper = TaskRemapper(param); + 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"); @@ -50,7 +54,8 @@ TEST_CASE("task_remapping_with_normal_and_wildcard") { R"( pick_and_place: [pick, "*"] )"; - auto remapper = TaskRemapper(param); + 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/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp index 441827c..6581a6a 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.cpp +++ b/nexus_system_orchestrator/src/system_orchestrator.cpp @@ -104,8 +104,9 @@ SystemOrchestrator::SystemOrchestrator(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 param = this->declare_parameter("remap_task_types", "", desc); - this->_task_remapper = std::make_shared(param); + const auto yaml = this->declare_parameter("remap_task_types", "", desc); + const auto remaps = YAML::Load(yaml); + this->_task_remapper = std::make_shared(remaps); } { 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 c404f13..3e1b4e3 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp @@ -93,7 +93,6 @@ WorkcellOrchestrator::WorkcellOrchestrator(const rclcpp::NodeOptions& options) desc.description = "A yaml containing a dictionary of task types and an array of remaps."; const auto param = this->declare_parameter("remap_task_types", "", desc); - this->_task_remapper = std::make_shared(param); } { ParameterDescriptor desc; @@ -685,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) {