From b881d1e2046602ce3f909ac9df26412bc5935e94 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 10 Jan 2025 17:34:35 +0800
Subject: [PATCH] Change API to std::optional

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 nexus_common/include/nexus_common/task_remapper.hpp       | 6 +++++-
 nexus_common/src/task_remapper.cpp                        | 4 ++--
 nexus_system_orchestrator/src/execute_task.cpp            | 6 +++---
 nexus_workcell_orchestrator/src/workcell_orchestrator.cpp | 8 +++++---
 4 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/nexus_common/include/nexus_common/task_remapper.hpp b/nexus_common/include/nexus_common/task_remapper.hpp
index ab7e9dc..15a3833 100644
--- a/nexus_common/include/nexus_common/task_remapper.hpp
+++ b/nexus_common/include/nexus_common/task_remapper.hpp
@@ -22,6 +22,9 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <optional>
+#include <string>
+
 namespace nexus::common {
 
 /**
@@ -37,8 +40,9 @@ class NEXUS_COMMON_EXPORT TaskRemapper
 
   /*
    * Remaps, if necessary, the input task
+   * Returns a value if the task was remapped, std::nullopt otherwise
    */
-  std::string remap(const std::string& task) const;
+  std::optional<std::string> remap(const std::string& task) const;
 
 private:
   // If present, match every incoming task to the target task
diff --git a/nexus_common/src/task_remapper.cpp b/nexus_common/src/task_remapper.cpp
index 8bf0f28..2e6b458 100644
--- a/nexus_common/src/task_remapper.cpp
+++ b/nexus_common/src/task_remapper.cpp
@@ -43,7 +43,7 @@ TaskRemapper::TaskRemapper(const std::string& param)
   }
 }
 
-std::string TaskRemapper::remap(const std::string& task) const
+std::optional<std::string> TaskRemapper::remap(const std::string& task) const
 {
   if (this->_wildcard_match.has_value())
   {
@@ -54,7 +54,7 @@ std::string TaskRemapper::remap(const std::string& task) const
   {
     return it->second;
   }
-  return task;
+  return std::nullopt;
 }
 
 
diff --git a/nexus_system_orchestrator/src/execute_task.cpp b/nexus_system_orchestrator/src/execute_task.cpp
index b20330f..f3ce222 100644
--- a/nexus_system_orchestrator/src/execute_task.cpp
+++ b/nexus_system_orchestrator/src/execute_task.cpp
@@ -42,15 +42,15 @@ BT::NodeStatus ExecuteTask::onStart()
   // Remap the BT filename to load if one is provided.
   std::string bt_name = task->type;
   const auto new_task = _ctx->task_remapper->remap(task->type);
-  if (new_task != task->type)
+  if (new_task.has_value())
   {
     RCLCPP_DEBUG(
       _ctx->node.get_logger(),
       "[ExecuteTask] Loading remapped BT [%s] for original task type [%s]",
-      new_task.c_str(),
+      new_task.value().c_str(),
       task->type.c_str()
     );
-    bt_name = new_task;
+    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))
diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
index 5c66eaf..c404f13 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
+++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
@@ -979,17 +979,19 @@ 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)
+  auto bt_name = ctx->task.type;
+  if (new_task.has_value())
   {
     RCLCPP_DEBUG(
       this->get_logger(),
       "Loading remapped BT [%s] for original task type [%s]",
-      new_task.c_str(),
+      new_task.value().c_str(),
       ctx->task.type.c_str()
     );
+    bt_name = new_task.value();
   }
   return this->_bt_factory->createTreeFromFile(this->_bt_store.get_bt(
-        new_task));
+        bt_name));
 }
 
 void WorkcellOrchestrator::_handle_command_success(