Change TaskRemapper API, remove duplicated implementation

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

nexus_common/include/nexus_common/task_remapper.hpp

@@ -22,6 +22,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <yaml-cpp/yaml.h>
+
 #include <optional>
 #include <string>
 
@@ -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

nexus_common/src/task_remapper.cpp

@@ -17,13 +17,10 @@
 
 #include "task_remapper.hpp"
 
-#include <yaml-cpp/yaml.h>
-
 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<std::string>();

nexus_common/src/task_remapper_test.cpp

@@ -18,6 +18,8 @@
 #define CATCH_CONFIG_MAIN
 #include <rmf_utils/catch.hpp>
 
+#include <yaml-cpp/yaml.h>
+
 #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");
 }

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<common::TaskRemapper>(param);
+    const auto yaml = this->declare_parameter("remap_task_types", "", desc);
+    const auto remaps = YAML::Load(yaml);
+    this->_task_remapper = std::make_shared<common::TaskRemapper>(remaps);
   }
 
   {

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

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<std::string, std::string> _remap_task_types;
 };
 
 } // namespace nexus::workcell_orchestrator

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<common::TaskRemapper>(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<std::string>(), it->first.as<std::string>());
-      }
-    }
+    this->_task_remapper = std::make_shared<common::TaskRemapper>(node);
   }
   catch (YAML::ParserException& e)
   {