Add task remapper

Signed-off-by: Luca Della Vedova <[email protected]>

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 <rclcpp/rclcpp.hpp>
+
+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<std::string> _wildcard_match;
+  std::unordered_map<std::string, std::string> _task_remaps;
+};
+
+}
+
+#endif

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 <yaml-cpp/yaml.h>
+
+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<std::string>();
+    const auto& mappings = n.second;
+    for (const auto& m : mappings)
+    {
+      auto mapping = m.as<std::string>();
+      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;
+}
+
+
+}

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 <rmf_utils/catch.hpp>
+
+#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");
+}
+
+}