Introduce a common TaskRemapper with wildcard support

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})

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");
+}
+
+}

nexus_system_orchestrator/src/context.hpp

@@ -20,6 +20,7 @@
 
 #include "session.hpp"
 
+#include <nexus_common/task_remapper.hpp>
 #include <nexus_common/models/work_order.hpp>
 #include <nexus_endpoints.hpp>
 #include <nexus_orchestrator_msgs/msg/task_state.hpp>
@@ -43,8 +44,7 @@ public: rclcpp_lifecycle::LifecycleNode& node;
 public: std::string job_id;
 public: WorkOrder wo;
 public: std::vector<WorkcellTask> tasks;
-public: std::shared_ptr<const std::unordered_map<std::string,
-    std::string>> task_remaps;
+public: std::shared_ptr<const common::TaskRemapper> task_remapper;
   /**
    * Map of task ids and their assigned workcell ids.
    */

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 != task->type)
   {
     RCLCPP_DEBUG(
       _ctx->node.get_logger(),
       "[ExecuteTask] Loading remapped BT [%s] for original task type [%s]",
-      it->second.c_str(),
+      new_task.c_str(),
       task->type.c_str()
     );
-    bt_name = it->second;
+    bt_name = new_task;
   }
   std::filesystem::path task_bt_path(this->_bt_path / (bt_name + ".xml"));
   if (!std::filesystem::is_regular_file(task_bt_path))

nexus_system_orchestrator/src/system_orchestrator.cpp

@@ -100,23 +100,12 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
   }
 
   {
-    _task_remaps =
-      std::make_shared<std::unordered_map<std::string, std::string>>();
     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<std::string>();
-      const auto& mappings = n.second;
-      for (const auto& m : mappings)
-      {
-        this->_task_remaps->emplace(m.as<std::string>(), task_type);
-      }
-    }
+    const auto param = this->declare_parameter("remap_task_types", "", desc);
+    this->_task_remapper = std::make_shared<common::TaskRemapper>(param);
   }
 
   {
@@ -545,7 +534,7 @@ void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal)
 
   // using `new` because make_shared does not work with aggregate initializer
   std::shared_ptr<Context> ctx{new Context{*this,
-      goal.order.id, wo, tasks, this->_task_remaps,
+      goal.order.id, wo, tasks, this->_task_remapper,
       std::unordered_map<std::string, std::string>{},
       this->_workcell_sessions,
       this->_transporter_sessions, {}, nullptr,

nexus_system_orchestrator/src/system_orchestrator.hpp

@@ -24,6 +24,7 @@
 #include <nexus_common/action_client_bt_node.hpp>
 #include <nexus_common/models/work_order.hpp>
 #include <nexus_common/sync_service_client.hpp>
+#include <nexus_common/task_remapper.hpp>
 #include <nexus_endpoints.hpp>
 #include <nexus_lifecycle_manager/lifecycle_manager.hpp>
 #include <nexus_orchestrator_msgs/msg/workcell_task.hpp>
@@ -96,8 +97,8 @@ class SystemOrchestrator : public
   std::unique_ptr<lifecycle_manager::LifecycleManager<>> _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<std::unordered_map<std::string, std::string>> _task_remaps;
+  // Manages task remapping
+  std::shared_ptr<common::TaskRemapper> _task_remapper;
   std::shared_ptr<OnSetParametersCallbackHandle> _param_cb_handle;
 
   /**

nexus_workcell_orchestrator/src/workcell_orchestrator.cpp

@@ -92,18 +92,8 @@ 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<std::string>();
-      const auto& mappings = n.second;
-      for (const auto& m : mappings)
-      {
-        this->_task_remaps.emplace(m.as<std::string>(), task_type);
-      }
-    }
+    const auto param = this->declare_parameter("remap_task_types", "", desc);
+    this->_task_remapper = std::make_shared<common::TaskRemapper>(param);
   }
   {
     ParameterDescriptor desc;
@@ -988,8 +978,18 @@ BT::Tree WorkcellOrchestrator::_create_bt(const std::shared_ptr<Context>& 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);
+  if (new_task != ctx->task.type)
+  {
+    RCLCPP_DEBUG(
+      this->get_logger(),
+      "Loading remapped BT [%s] for original task type [%s]",
+      new_task.c_str(),
+      ctx->task.type.c_str()
+    );
+  }
   return this->_bt_factory->createTreeFromFile(this->_bt_store.get_bt(
-        ctx->task.type));
+        new_task));
 }
 
 void WorkcellOrchestrator::_handle_command_success(

nexus_workcell_orchestrator/src/workcell_orchestrator.hpp

@@ -28,6 +28,7 @@
 
 #include <nexus_common/action_client_bt_node.hpp>
 #include <nexus_common/bt_store.hpp>
+#include <nexus_common/task_remapper.hpp>
 
 #include <behaviortree_cpp_v3/action_node.h>
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -124,8 +125,8 @@ private: std::list<std::shared_ptr<Context>> _ctxs;
 private: std::shared_ptr<ContextManager> _ctx_mgr;
 private: std::unique_ptr<lifecycle_manager::LifecycleManager<>> _lifecycle_mgr{
     nullptr};
-// mapping of mapped task type and the original
-private: std::unordered_map<std::string, std::string> _task_remaps;
+// Takes care of remapping tasks
+private: std::shared_ptr<common::TaskRemapper> _task_remapper;
 private: TaskParser _task_parser;
 private: pluginlib::ClassLoader<TaskChecker> _task_checker_loader;
 private: std::shared_ptr<TaskChecker> _task_checker;