From a8c5bf433f478ad18b5d908d42e661e55c4e59c3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 14:50:56 +0800 Subject: [PATCH] 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"); +} + +}