Merge branch 'main' into luca/rmf_transporter

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,68 @@
+/*
+ * Copyright (C) 2025 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>
+
+#include <yaml-cpp/yaml.h>
+
+#include <optional>
+#include <string>
+
+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.
+   *
+   * 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".
+   * Note: If the value is specified as an asterisk, "*", any value will be
+   *  remapped to the specified key.
+   */
+  TaskRemapper(const YAML::Node& remaps);
+
+  /*
+   * Remaps, if necessary, the input task
+   * Returns a value if the task was remapped, std::nullopt otherwise
+   */
+  std::optional<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,58 @@
+/*
+ * Copyright (C) 2025 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"
+
+namespace nexus::common {
+
+TaskRemapper::TaskRemapper(const YAML::Node& remaps)
+{
+  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::optional<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 std::nullopt;
+}
+
+
+}

nexus_common/src/task_remapper_test.cpp

@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2025 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.
+ *
+ */
+
+#define CATCH_CONFIG_MAIN
+#include <rmf_utils/catch.hpp>
+
+#include <yaml-cpp/yaml.h>
+
+#include "task_remapper.hpp"
+
+namespace nexus::common::test {
+
+TEST_CASE("task_remapping") {
+  std::string param =
+    R"(
+      pick_and_place: [pick, place]
+    )";
+  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);
+}
+
+TEST_CASE("task_remapping_with_wildcard") {
+  std::string param =
+    R"(
+      pick_and_place: [pick, place]
+      main : ["*"]
+    )";
+  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");
+}
+
+TEST_CASE("task_remapping_with_normal_and_wildcard") {
+  std::string param =
+    R"(
+      pick_and_place: [pick, "*"]
+    )";
+  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/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.has_value())
   {
     RCLCPP_DEBUG(
       _ctx->node.get_logger(),
       "[ExecuteTask] Loading remapped BT [%s] for original task type [%s]",
-      it->second.c_str(),
+      new_task.value().c_str(),
       task->type.c_str()
     );
-    bt_name = it->second;
+    bt_name = new_task.value();
   }
   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

@@ -101,23 +101,11 @@ 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);
   }
 
   {
@@ -174,6 +162,21 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
 auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous)
 -> CallbackReturn
 {
+  // Create map for remapping capabilities.
+  const std::string remap_caps = this->get_parameter("remap_task_types").as_string();
+  try
+  {
+    YAML::Node node = YAML::Load(remap_caps);
+    this->_task_remapper = std::make_shared<common::TaskRemapper>(std::move(node));
+  }
+  catch (YAML::ParserException& e)
+  {
+    RCLCPP_ERROR(
+      this->get_logger(), "Failed to parse remap_task_types parameter: (%s)",
+      e.what());
+    return CallbackReturn::FAILURE;
+  }
+
   // create list workcells service
   this->_list_workcells_srv =
     this->create_service<endpoints::ListWorkcellsService::ServiceType>(
@@ -580,7 +583,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>
@@ -98,8 +99,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/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