From 90edc61331a82c61c639f24b9f575a99c572fc58 Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Wed, 8 Nov 2023 11:10:35 +0800
Subject: [PATCH 01/12] error type

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 nexus_common/include/nexus_common/error.hpp | 79 +++++++++++++++++++++
 1 file changed, 79 insertions(+)
 create mode 100644 nexus_common/include/nexus_common/error.hpp

diff --git a/nexus_common/include/nexus_common/error.hpp b/nexus_common/include/nexus_common/error.hpp
new file mode 100644
index 0000000..926e61c
--- /dev/null
+++ b/nexus_common/include/nexus_common/error.hpp
@@ -0,0 +1,79 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef NEXUS_COMMON__ERROR_HPP
+#define NEXUS_COMMON__ERROR_HPP
+
+#include <exception>
+#include <memory>
+#include <type_traits>
+#include <variant>
+
+namespace nexus::common {
+
+/**
+ * Represents the result of a function call that may return an error.
+ *
+ * Example usage:
+ * ```cpp
+ * const auto& result = foo();
+ * if (result.error()) {
+ *   // handle error
+ * }
+ * // use `result.value()` to get pointer to result value.
+ * ```
+ */
+template<typename T, typename E = std::exception>
+class Result
+{
+public: Result(T t)
+  : _var(std::move(t)) {}
+
+public: Result(std::shared_ptr<E> e)
+  : _var(std::move(e)) {}
+
+public:
+  template<typename DE, typename = std::enable_if_t<std::is_base_of_v<E, DE>>>
+  Result(DE de)
+  : _var(std::make_shared<DE>(std::move(de))) {}
+
+public: T* value()
+  {
+    return std::get_if<T>(this->_var);
+  }
+
+public: const T* value() const
+  {
+    return std::get_if<T>(this->_var);
+  }
+
+public: E* error()
+  {
+    return std::get_if<std::shared_ptr<E>>(this->_var).data();
+  }
+
+public: const E* error() const
+  {
+    return std::get_if<std::shared_ptr<E>>(this->_var).data();
+  }
+
+private: std::variant<T, std::shared_ptr<E>> _var;
+};
+
+}
+
+#endif

From d3050535696a0bbda32e57490daa6ae9a1ccec64 Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Thu, 9 Nov 2023 12:36:56 +0800
Subject: [PATCH 02/12] remove exceptions in JobManager

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 nexus_common/CMakeLists.txt                   |   7 +-
 nexus_common/include/nexus_common/error.hpp   |  73 ++++++++-
 nexus_common/src/error_test.cpp               |  52 +++++++
 .../src/get_joint_constraints.cpp             |   2 +
 .../src/job_manager.cpp                       | 145 ++++++++++++------
 .../src/job_manager.hpp                       |  19 ++-
 .../src/job_manager_test.cpp                  |  26 ++--
 .../src/workcell_orchestrator.cpp             |  51 +++---
 8 files changed, 264 insertions(+), 111 deletions(-)
 create mode 100644 nexus_common/src/error_test.cpp

diff --git a/nexus_common/CMakeLists.txt b/nexus_common/CMakeLists.txt
index ef46734..3701b90 100644
--- a/nexus_common/CMakeLists.txt
+++ b/nexus_common/CMakeLists.txt
@@ -118,17 +118,18 @@ if(BUILD_TESTING)
   endfunction()
 
   nexus_add_test(action_client_bt_node_test src/action_client_bt_node_test.cpp)
-  nexus_add_test(pausable_sequence_test src/pausable_sequence_test.cpp)
-  nexus_add_test(service_client_bt_node_test src/service_client_bt_node_test.cpp)
+  nexus_add_test(error_test src/main_test.cpp src/error_test.cpp)
+  nexus_add_test(logging_test src/logging_test.cpp)
   nexus_add_test(models_test
     src/main_test.cpp
     src/models/work_order_test.cpp
   )
+  nexus_add_test(pausable_sequence_test src/pausable_sequence_test.cpp)
   nexus_add_test(ros_utils_test
     src/main_test.cpp
     src/batch_service_call_test.cpp
   )
-  nexus_add_test(logging_test src/logging_test.cpp)
+  nexus_add_test(service_client_bt_node_test src/service_client_bt_node_test.cpp)
   nexus_add_test(sync_ros_client_test src/sync_ros_client_test.cpp)
 endif()
 
diff --git a/nexus_common/include/nexus_common/error.hpp b/nexus_common/include/nexus_common/error.hpp
index 926e61c..dd3d57e 100644
--- a/nexus_common/include/nexus_common/error.hpp
+++ b/nexus_common/include/nexus_common/error.hpp
@@ -20,6 +20,8 @@
 
 #include <exception>
 #include <memory>
+#include <optional>
+#include <string>
 #include <type_traits>
 #include <variant>
 
@@ -46,34 +48,89 @@ public: Result(T t)
 public: Result(std::shared_ptr<E> e)
   : _var(std::move(e)) {}
 
-public:
-  template<typename DE, typename = std::enable_if_t<std::is_base_of_v<E, DE>>>
-  Result(DE de)
-  : _var(std::make_shared<DE>(std::move(de))) {}
+public: template<typename D,
+    typename = std::enable_if_t<std::is_base_of_v<E, D>>>
+  Result(D d)
+  : _var(std::make_shared<D>(std::move(d))) {}
 
 public: T* value()
   {
-    return std::get_if<T>(this->_var);
+    return std::get_if<T>(&this->_var);
   }
 
 public: const T* value() const
   {
-    return std::get_if<T>(this->_var);
+    return std::get_if<T>(&this->_var);
+  }
+
+public: T* value_or_abort()
+  {
+    if (!this->value())
+    {
+      std::abort();
+    }
+    return this->value();
+  }
+
+public: const T* value_or_abort() const
+  {
+    if (!this->value())
+    {
+      std::abort();
+    }
+    return this->value();
   }
 
 public: E* error()
   {
-    return std::get_if<std::shared_ptr<E>>(this->_var).data();
+    auto* maybe_err = std::get_if<std::shared_ptr<E>>(&this->_var);
+    return maybe_err ? maybe_err->get() : nullptr;
   }
 
 public: const E* error() const
   {
-    return std::get_if<std::shared_ptr<E>>(this->_var).data();
+    auto* maybe_err = std::get_if<std::shared_ptr<E>>(&this->_var);
+    return maybe_err ? maybe_err->get() : nullptr;
   }
 
 private: std::variant<T, std::shared_ptr<E>> _var;
 };
 
+template<typename E>
+class Result<void, E>
+{
+public: Result()
+  : _var(std::nullopt) {}
+
+public: Result(std::shared_ptr<E> e)
+  : _var(std::move(e)) {}
+
+public: template<typename D,
+    typename = std::enable_if_t<std::is_base_of_v<E, D>>>
+  Result(D d)
+  : _var(std::make_shared<D>(std::move(d))) {}
+
+public: void value_or_abort() const
+  {
+    if (this->error())
+    {
+      std::abort();
+    }
+  }
+
+public: E* error()
+  {
+    return this->_var.has_value() ? this->_var->get() : nullptr;
+  }
+
+public: const E* error() const
+  {
+    return this->_var.has_value() ? this->_var->get() : nullptr;
+  }
+
+private: std::optional<std::shared_ptr<E>> _var;
+};
+
 }
 
 #endif
diff --git a/nexus_common/src/error_test.cpp b/nexus_common/src/error_test.cpp
new file mode 100644
index 0000000..f915e08
--- /dev/null
+++ b/nexus_common/src/error_test.cpp
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2022 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.
+ *
+ */
+
+#include <catch2/catch.hpp>
+
+#include "nexus_common/error.hpp"
+
+namespace nexus::common::test {
+
+TEST_CASE("get_result_value_and_error") {
+  Result<int> good(1);
+  CHECK(*good.value() == 1);
+  CHECK(!good.error());
+
+  const Result<int> const_good(1);
+  CHECK(*const_good.value() == 1);
+  CHECK(!const_good.error());
+
+  Result<int> bad(std::make_shared<std::runtime_error>("bad"));
+  CHECK(!bad.value());
+  CHECK(bad.error());
+
+  Result<int> bad_val_constructor(std::runtime_error("bad_val_constructor"));
+  CHECK(!bad_val_constructor.value());
+  CHECK(bad_val_constructor.error());
+
+  Result<void> void_good;
+  CHECK(!void_good.error());
+
+  Result<void> void_bad(std::make_shared<std::runtime_error>("void_bad"));
+  CHECK(void_bad.error());
+
+  Result<void> void_bad_val_constructor(std::runtime_error(
+      "void_bad_val_constructor"));
+  CHECK(void_bad_val_constructor.error());
+}
+
+}
diff --git a/nexus_workcell_orchestrator/src/get_joint_constraints.cpp b/nexus_workcell_orchestrator/src/get_joint_constraints.cpp
index 364ca11..c5dfe04 100644
--- a/nexus_workcell_orchestrator/src/get_joint_constraints.cpp
+++ b/nexus_workcell_orchestrator/src/get_joint_constraints.cpp
@@ -69,6 +69,8 @@ BT::NodeStatus GetJointConstraints::tick()
   }
 
   // Convert TrajectoryPoint to set of JointConstraints.
+  // `index` is checked above so the accesses here should never cause OOB.
+  // No need to catch exception.
   std::vector<moveit_msgs::msg::JointConstraint> constraints;
   const auto& final_joint_values = joint_trajectory.points.at(index).positions;
   for (std::size_t i = 0; i < final_joint_values.size(); ++i)
diff --git a/nexus_workcell_orchestrator/src/job_manager.cpp b/nexus_workcell_orchestrator/src/job_manager.cpp
index c893b16..3058db9 100644
--- a/nexus_workcell_orchestrator/src/job_manager.cpp
+++ b/nexus_workcell_orchestrator/src/job_manager.cpp
@@ -76,10 +76,8 @@ void JobManager::_tick_bt(Job& job)
     }
     default: {
       std::ostringstream oss;
-      oss << "Error ticking task [" << job.ctx->task.id <<
-        "]: Behavior tree returned invalid or unknown status [" <<
-        bt_status << "]";
-      RCLCPP_ERROR_STREAM(job.ctx->node->get_logger(), oss.str());
+      oss << "behavior tree returned invalid or unknown status (" <<
+        bt_status << ")";
       throw std::runtime_error(oss.str());
     }
   }
@@ -120,7 +118,7 @@ uint8_t JobManager::_tick_job(Job& job)
   return task_status;
 }
 
-Job& JobManager::assign_task(
+common::Result<Job*> JobManager::assign_task(
   const std::string& task_id)
 {
   const auto it =
@@ -131,7 +129,11 @@ Job& JobManager::assign_task(
       });
   if (it != this->_jobs.end())
   {
-    throw JobError("Another task with the same id already exist");
+    std::string msg("Another task with the same id already exist");
+    RCLCPP_ERROR(
+      this->_node->get_logger(), "Failed to assign task [%s]: %s",
+      task_id.c_str(), msg.c_str());
+    return JobError(msg);
   }
 
   auto& j = this->_jobs.emplace_back(Job{nullptr, std::nullopt, nullptr,
@@ -149,10 +151,11 @@ Job& JobManager::assign_task(
   RCLCPP_INFO(
     this->_node->get_logger(), "Assigned task [%s]",
     task_id.c_str());
-  return j;
+  return &j;
 }
 
-Job& JobManager::queue_task(const GoalHandlePtr& goal_handle,
+common::Result<Job*> JobManager::queue_task(
+  const GoalHandlePtr& goal_handle,
   const std::shared_ptr<Context>& ctx, BT::Tree&& bt)
 {
   const auto& job_id = goal_handle->get_goal()->task.id;
@@ -161,14 +164,15 @@ Job& JobManager::queue_task(const GoalHandlePtr& goal_handle,
       {
         return j.task_state.task_id == job_id;
       });
-  if (it == this->_jobs.end())
+
+  if (it == this->_jobs.end() ||
+    it->task_state.status != TaskState::STATUS_ASSIGNED)
   {
-    // abort the goal and throw error
-    const auto result = std::make_shared<WorkcellRequest::Result>();
-    result->success = false;
-    result->message = "Task not found";
-    goal_handle->abort(result);
-    throw JobError("Task not found");
+    std::string msg("Task is not assigned");
+    RCLCPP_ERROR(
+      this->_node->get_logger(), "Failed to queue task [%s]: %s",
+      job_id.c_str(), msg.c_str());
+    return JobError(msg);
   }
 
   it->ctx = ctx;
@@ -180,10 +184,11 @@ Job& JobManager::queue_task(const GoalHandlePtr& goal_handle,
   RCLCPP_INFO(
     it->ctx->node->get_logger(), "Task [%s] is now queued",
     it->task_state.task_id.c_str());
-  return *it;
+  return &*it;
 }
 
-void JobManager::remove_assigned_task(const std::string& task_id)
+common::Result<void> JobManager::remove_assigned_task(
+  const std::string& task_id)
 {
   const auto it = std::find_if(this->_jobs.begin(),
       this->_jobs.end(), [&task_id](const Job& j)
@@ -192,66 +197,112 @@ void JobManager::remove_assigned_task(const std::string& task_id)
       });
   if (it == this->_jobs.end())
   {
-    return;
+    return common::Result<void>();
   }
   if (it->task_state.status != TaskState::STATUS_ASSIGNED)
   {
-    throw JobError("Task is not assigned");
+    std::string msg("Task is not assigned");
+    RCLCPP_ERROR(
+      this->_node->get_logger(), "Failed to remove task [%s]: %s",
+      task_id.c_str(), msg.c_str());
+    return JobError(msg);
   }
   this->_jobs.erase(it);
+  return common::Result<void>();
 }
 
-void JobManager::halt_all_jobs()
+common::Result<void> JobManager::halt_all_jobs()
 {
+  const auto handle_exception = [this](const std::exception& e, const Job& j)
+    {
+      RCLCPP_ERROR(
+        this->_node->get_logger(), "Failed to halt job [%s]: %s",
+        j.task_state.task_id.c_str(), e.what());
+      return e;
+    };
+
   RCLCPP_INFO(this->_node->get_logger(), "Halting all tasks");
   for (auto& j : this->_jobs)
   {
-    // halt bts that are running
-    if (j.task_state.status == TaskState::STATUS_RUNNING)
+    try
     {
-      this->_ctx_mgr->set_active_context(j.ctx);
-      j.bt->haltTree();
-      this->_ctx_mgr->set_active_context(nullptr);
+      // halt bts that are running
+      if (j.task_state.status == TaskState::STATUS_RUNNING)
+      {
+        this->_ctx_mgr->set_active_context(j.ctx);
+        j.bt->haltTree();
+        this->_ctx_mgr->set_active_context(nullptr);
 
-      RCLCPP_INFO(
-        j.ctx->node->get_logger(), "Task [%s] halted", j.ctx->task.id.c_str());
-      // Publish feedback.
-      j.task_state.status = TaskState::STATUS_FAILED;
-      auto fb = std::make_shared<WorkcellRequest::Feedback>();
-      fb->state = j.task_state;
-      j.goal_handle->publish_feedback(std::move(fb));
+        RCLCPP_INFO(
+          j.ctx->node->get_logger(), "Task [%s] halted",
+          j.ctx->task.id.c_str());
+        // Publish feedback.
+        j.task_state.status = TaskState::STATUS_FAILED;
+        auto fb = std::make_shared<WorkcellRequest::Feedback>();
+        fb->state = j.task_state;
+        j.goal_handle->publish_feedback(std::move(fb));
 
-      // Abort the action request.
-      auto result =
-        std::make_shared<endpoints::WorkcellRequestAction::ActionType::Result>();
-      result->success = false;
-      result->message = "Task halted";
-      j.goal_handle->abort(result);
-      break;
+        // Abort the action request.
+        auto result =
+          std::make_shared<endpoints::WorkcellRequestAction::ActionType::Result>();
+        result->success = false;
+        result->message = "Task halted";
+        j.goal_handle->abort(result);
+        break;
+      }
+    }
+    catch (const rclcpp::exceptions::RCLError& e)
+    {
+      return handle_exception(e, j);
+    }
+    catch (const std::runtime_error& e)
+    {
+      return handle_exception(e, j);
     }
   }
 
   this->_jobs.clear();
+  return common::Result<void>();
 }
 
-void JobManager::tick()
+common::Result<void> JobManager::tick()
 {
+  const auto handle_exception = [this](const std::exception& e, const Job& j)
+    {
+      RCLCPP_ERROR(
+        this->_node->get_logger(), "Error ticking task [%s]: %s",
+        j.task_state.task_id.c_str(), e.what());
+      return e;
+    };
+
   size_t i = 0;
   auto it = this->_jobs.begin();
   for (; i < this->_max_concurrent && it != this->_jobs.end(); ++i)
   {
-    const auto task_status = this->_tick_job(*it);
-    if (task_status == TaskState::STATUS_FAILED ||
-      task_status == TaskState::STATUS_FINISHED)
+    try
+    {
+      const auto task_status = this->_tick_job(*it);
+      if (task_status == TaskState::STATUS_FAILED ||
+        task_status == TaskState::STATUS_FINISHED)
+      {
+        it = this->_jobs.erase(it);
+        continue;
+      }
+      else
+      {
+        ++it;
+      }
+    }
+    catch (const rclcpp::exceptions::RCLError& e)
     {
-      it = this->_jobs.erase(it);
-      continue;
+      return handle_exception(e, *it);
     }
-    else
+    catch (const std::runtime_error& e)
     {
-      ++it;
+      return handle_exception(e, *it);
     }
   }
+  return common::Result<void>();
 }
 
 }
diff --git a/nexus_workcell_orchestrator/src/job_manager.hpp b/nexus_workcell_orchestrator/src/job_manager.hpp
index d00209a..f9a7fd2 100644
--- a/nexus_workcell_orchestrator/src/job_manager.hpp
+++ b/nexus_workcell_orchestrator/src/job_manager.hpp
@@ -21,6 +21,7 @@
 #include "job.hpp"
 
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <rclcpp_action/rclcpp_action.hpp>
@@ -54,29 +55,27 @@ public: JobManager(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
 
   /**
    * Assigns a task.
-   * @throw JobError
   */
-public: Job& assign_task(const std::string& task_id);
+public: [[nodiscard]] common::Result<Job*> assign_task(
+    const std::string& task_id);
 
   /**
-   * Queue a task. The task must already be assigned. If there is an error, the goal will
-   * immediately be aborted.
-   * @throw JobError
+   * Queue a task. The task must already be assigned.
    */
-public: Job& queue_task(const GoalHandlePtr& goal_handle,
+public: [[nodiscard]] common::Result<Job*> queue_task(
+    const GoalHandlePtr& goal_handle,
     const std::shared_ptr<Context>& ctx, BT::Tree&& bt);
 
   /**
    * Remove a assigned task.
-   * @throw JobError
    */
-public: void remove_assigned_task(const std::string& task_id);
+public: [[nodiscard]] common::Result<void> remove_assigned_task(const std::string& task_id);
 
   /**
    * Forcefully stop and clear all jobs. Jobs will be stopped as soon as possible, unlike
    * `cancel_all_jobs`, the behavior trees cannot react to this event.
    */
-public: void halt_all_jobs();
+public: [[nodiscard]] common::Result<void> halt_all_jobs();
 
   /**
    * TODO: Implement on cancel bt node.
@@ -88,7 +87,7 @@ public: void halt_all_jobs();
   /**
    * Tick all active jobs.
    */
-public: void tick();
+public: [[nodiscard]] common::Result<void> tick();
 
 public: const std::list<Job>& jobs() const
   {
diff --git a/nexus_workcell_orchestrator/src/job_manager_test.cpp b/nexus_workcell_orchestrator/src/job_manager_test.cpp
index 121dc48..2db2e78 100644
--- a/nexus_workcell_orchestrator/src/job_manager_test.cpp
+++ b/nexus_workcell_orchestrator/src/job_manager_test.cpp
@@ -64,15 +64,15 @@ TEST_CASE("JobManager") {
   fixture.spin_in_background();
 
   SECTION("cannot assign same task twice") {
-    CHECK_NOTHROW(job_mgr.assign_task("test"));
-    CHECK_THROWS_AS(job_mgr.assign_task("test"), JobError);
+    CHECK(job_mgr.assign_task("test").value());
+    CHECK(job_mgr.assign_task("test").error());
   }
 
   SECTION("cannot queue task that is not assigned") {
     handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle)
       {
-        CHECK_THROWS_AS(job_mgr.queue_task(goal_handle, nullptr,
-          bt_factory.createTreeFromText(bt)), JobError);
+        CHECK(job_mgr.queue_task(goal_handle, nullptr,
+          bt_factory.createTreeFromText(bt)).error());
       };
 
     WorkcellTask::Goal goal;
@@ -91,7 +91,7 @@ TEST_CASE("JobManager") {
 
   SECTION("assign task sets job data correctly") {
     Job* job;
-    job = &job_mgr.assign_task("test");
+    job = *job_mgr.assign_task("test").value();
     CHECK(!job->bt.has_value());
     CHECK(!job->bt_logging);
     CHECK(job->ctx == nullptr);
@@ -102,7 +102,7 @@ TEST_CASE("JobManager") {
     CHECK(job->tick_count == 0);
 
     SECTION("ticking assigned but unqueued task is a noop") {
-      job_mgr.tick();
+      job_mgr.tick().value_or_abort();
       CHECK(job->tick_count == 0);
     }
 
@@ -110,8 +110,8 @@ TEST_CASE("JobManager") {
       handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle)
         {
           auto ctx = std::make_shared<Context>(fixture.node);
-          CHECK(&job_mgr.queue_task(goal_handle, ctx,
-            bt_factory.createTreeFromText(bt)) == job);
+          CHECK(*job_mgr.queue_task(goal_handle, ctx,
+            bt_factory.createTreeFromText(bt)).value() == job);
           CHECK(job->bt.has_value());
           CHECK(job->bt_logging);
           REQUIRE(job->ctx == ctx);
@@ -141,7 +141,7 @@ TEST_CASE("JobManager") {
     std::vector<std::string> task_ids{"test1", "test2", "test3"};
     for (const auto& task_id : task_ids)
     {
-      job_mgr.assign_task(task_id);
+      job_mgr.assign_task(task_id).value_or_abort();
     }
 
     std::promise<void> done;
@@ -149,8 +149,8 @@ TEST_CASE("JobManager") {
     handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle)
       {
         auto ctx = std::make_shared<Context>(fixture.node);
-        REQUIRE_NOTHROW(job_mgr.queue_task(goal_handle, ctx,
-          bt_factory.createTreeFromText(bt)));
+        REQUIRE(job_mgr.queue_task(goal_handle, ctx,
+          bt_factory.createTreeFromText(bt)).value());
         if (++queued == task_ids.size())
         {
           done.set_value();
@@ -168,11 +168,11 @@ TEST_CASE("JobManager") {
     REQUIRE(done.get_future().wait_for(std::chrono::seconds(
         1)) == std::future_status::ready);
 
-    job_mgr.tick();
+    job_mgr.tick().value_or_abort();
     // test1 and test2 should be finished
     CHECK(job_mgr.jobs().size() == 1);
 
-    job_mgr.tick();
+    job_mgr.tick().value_or_abort();
     // test3 should now be finished
     CHECK(job_mgr.jobs().size() == 0);
   }
diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
index 3512fb1..ec5ea73 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
+++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
@@ -214,7 +214,7 @@ auto WorkcellOrchestrator::on_activate(
   RCLCPP_INFO(this->get_logger(), "Workcell activated");
   this->_bt_timer = this->create_wall_timer(BT_TICK_RATE, [this]()
       {
-        this->_job_mgr.value().tick();
+        this->_job_mgr.value().tick().value_or_abort();
       });
   return CallbackReturn::SUCCESS;
 }
@@ -224,7 +224,7 @@ auto WorkcellOrchestrator::on_deactivate(
 {
   RCLCPP_INFO(this->get_logger(),
     "Halting ongoing task before deactivating workcell");
-  this->_job_mgr->halt_all_jobs();
+  this->_job_mgr->halt_all_jobs().value_or_abort();
 
   this->_bt_timer->cancel();
   this->_bt_timer.reset();
@@ -245,7 +245,7 @@ auto WorkcellOrchestrator::on_cleanup(
   this->_signal_wc_srv.reset();
   this->_task_doable_srv.reset();
   this->_cmd_server.reset();
-  this->_job_mgr->halt_all_jobs();
+  this->_job_mgr->halt_all_jobs().value_or_abort();
   RCLCPP_INFO(this->get_logger(), "Cleaned up");
   return CallbackReturn::SUCCESS;
 }
@@ -301,7 +301,7 @@ auto WorkcellOrchestrator::_configure(
           "Cancelling specific task is no longer supported, all tasks will be cancelled together to ensure line consistency");
       }
 
-      this->_job_mgr->halt_all_jobs();
+      this->_job_mgr->halt_all_jobs().value_or_abort();
       return rclcpp_action::CancelResponse::ACCEPT;
     },
     [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<endpoints::WorkcellRequestAction::ActionType>>
@@ -319,14 +319,14 @@ auto WorkcellOrchestrator::_configure(
       auto ctx = std::make_shared<Context>(this->shared_from_this());
       ctx->task = this->_task_parser.parse_task(goal_handle->get_goal()->task);
       auto bt = this->_create_bt(ctx);
-      try
+      auto r = this->_job_mgr->queue_task(goal_handle, ctx, std::move(bt));
+      if (r.error())
       {
-        this->_job_mgr->queue_task(goal_handle, ctx, std::move(bt));
-      }
-      catch (const JobError& e)
-      {
-        RCLCPP_ERROR(this->get_logger(), "Failed to queue task [%s]: %s",
-        goal_handle->get_goal()->task.id.c_str(), e.what());
+        auto result = std::make_shared<endpoints::WorkcellRequestAction::ActionType::Result>();
+        result->success = false;
+        result->message = r.error()->what();
+        goal_handle->abort(result);
+        return;
       }
     });
 
@@ -380,18 +380,14 @@ auto WorkcellOrchestrator::_configure(
       ConstSharedPtr req,
       endpoints::QueueWorkcellTaskService::ServiceType::Response::SharedPtr resp)
       {
-        try
-        {
-          this->_job_mgr->assign_task(req->task_id);
-          resp->success = true;
-        }
-        catch (const JobError& e)
+        auto r = this->_job_mgr->assign_task(req->task_id);
+        if (r.error())
         {
           resp->success = false;
-          resp->message = e.what();
-          RCLCPP_ERROR(this->get_logger(), "Failed to assign task [%s]: %s",
-          req->task_id.c_str(), e.what());
+          resp->message = r.error()->what();
+          return;
         }
+        resp->success = true;
       });
 
   this->_remove_pending_task_srv =
@@ -403,19 +399,14 @@ auto WorkcellOrchestrator::_configure(
       {
         RCLCPP_DEBUG(this->get_logger(),
         "received request to remove pending task [%s]", req->task_id.c_str());
-        try
-        {
-          this->_job_mgr->remove_assigned_task(req->task_id);
-          resp->success = true;
-        }
-        catch (const JobError& e)
+        auto r = this->_job_mgr->remove_assigned_task(req->task_id);
+        if (r.error())
         {
-          RCLCPP_WARN(this->get_logger(),
-          "Failed to remove assigned task [%s]: %s", req->task_id.c_str(),
-          e.what());
           resp->success = false;
-          resp->message = e.what();
+          resp->message = r.error()->what();
+          return;
         }
+        resp->success = true;
       });
 
   this->_tf2_buffer = std::make_shared<tf2_ros::Buffer>(this->get_clock());

From 743557aa64975673e4f0c06bce7a1481af286918 Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Thu, 9 Nov 2023 14:31:14 +0800
Subject: [PATCH 03/12] remove exceptions in TaskParser

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 .../src/task_parser.cpp                       | 21 +++++++++-----
 .../src/task_parser.hpp                       |  4 +--
 .../src/workcell_orchestrator.cpp             | 29 +++++++++++++++----
 3 files changed, 39 insertions(+), 15 deletions(-)

diff --git a/nexus_workcell_orchestrator/src/task_parser.cpp b/nexus_workcell_orchestrator/src/task_parser.cpp
index f3926b9..5cf0f3b 100644
--- a/nexus_workcell_orchestrator/src/task_parser.cpp
+++ b/nexus_workcell_orchestrator/src/task_parser.cpp
@@ -24,15 +24,22 @@
 namespace nexus::workcell_orchestrator {
 
 //==============================================================================
-Task TaskParser::parse_task(
+common::Result<Task> TaskParser::parse_task(
   const nexus_orchestrator_msgs::msg::WorkcellTask& workcell_task)
 {
-  return Task{
-    workcell_task.id,
-    this->remap_task_type(workcell_task.type),
-    YAML::Load(workcell_task.payload),
-    YAML::Load(workcell_task.previous_results),
-  };
+  try
+  {
+    return Task{
+      workcell_task.id,
+      this->remap_task_type(workcell_task.type),
+      YAML::Load(workcell_task.payload),
+      YAML::Load(workcell_task.previous_results),
+    };
+  }
+  catch (const YAML::ParserException& e)
+  {
+    return e;
+  }
 }
 
 //==============================================================================
diff --git a/nexus_workcell_orchestrator/src/task_parser.hpp b/nexus_workcell_orchestrator/src/task_parser.hpp
index b93cf46..e914ba7 100644
--- a/nexus_workcell_orchestrator/src/task_parser.hpp
+++ b/nexus_workcell_orchestrator/src/task_parser.hpp
@@ -19,6 +19,7 @@
 #define NEXUS_CAPABILITITY_PLUGIN__TASK_PARSER_HPP
 
 #include <nexus_capabilities/task.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_orchestrator_msgs/msg/workcell_task.hpp>
 #include <unordered_map>
 
@@ -34,9 +35,8 @@ public: TaskParser() {}
 
   /**
    * Parses a workcell task.
-   * @throws YAML::Exception if parsing the payload fails.
    */
-public: Task parse_task(const nexus_orchestrator_msgs::msg::WorkcellTask& task);
+public: common::Result<Task> parse_task(const nexus_orchestrator_msgs::msg::WorkcellTask& task);
 
   /**
    * Add task type to remap to.
diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
index ec5ea73..b588fe9 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
+++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
@@ -317,14 +317,26 @@ auto WorkcellOrchestrator::_configure(
       }
 
       auto ctx = std::make_shared<Context>(this->shared_from_this());
-      ctx->task = this->_task_parser.parse_task(goal_handle->get_goal()->task);
+      auto task_result =
+      this->_task_parser.parse_task(goal_handle->get_goal()->task);
+      if (task_result.error())
+      {
+        auto result = std::make_shared<endpoints::WorkcellRequestAction::ActionType::Result>();
+        result->success = false;
+        result->message = task_result.error()->what();
+        goal_handle->abort(result);
+        return;
+      }
+      ctx->task = *task_result.value();
+
       auto bt = this->_create_bt(ctx);
-      auto r = this->_job_mgr->queue_task(goal_handle, ctx, std::move(bt));
-      if (r.error())
+      auto job_result =
+      this->_job_mgr->queue_task(goal_handle, ctx, std::move(bt));
+      if (job_result.error())
       {
         auto result = std::make_shared<endpoints::WorkcellRequestAction::ActionType::Result>();
         result->success = false;
-        result->message = r.error()->what();
+        result->message = job_result.error()->what();
         goal_handle->abort(result);
         return;
       }
@@ -727,8 +739,13 @@ void WorkcellOrchestrator::_handle_task_doable(
   RCLCPP_DEBUG(this->get_logger(), "Got request to check task doable");
   try
   {
-    auto task = this->_task_parser.parse_task(req->task);
-    resp->success = this->_can_perform_task(task);
+    auto r = this->_task_parser.parse_task(req->task);
+    if (r.error())
+    {
+      resp->success = false;
+      return;
+    }
+    resp->success = this->_can_perform_task(*r.value());
     if (resp->success)
     {
       RCLCPP_DEBUG(this->get_logger(), "Workcell can perform task");

From 8a21fe876ac26bb0eb339876aa84f4f9662c55d5 Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Thu, 9 Nov 2023 15:44:10 +0800
Subject: [PATCH 04/12] remove exceptions

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 .../include/nexus_common/action_client_bt_node.hpp    | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/nexus_common/include/nexus_common/action_client_bt_node.hpp b/nexus_common/include/nexus_common/action_client_bt_node.hpp
index 4b38bc9..f65d74d 100644
--- a/nexus_common/include/nexus_common/action_client_bt_node.hpp
+++ b/nexus_common/include/nexus_common/action_client_bt_node.hpp
@@ -178,7 +178,16 @@ BT::NodeStatus ActionClientBtNode<NodePtrT, ActionT>::onStart()
   RCLCPP_DEBUG(
     this->_node->get_logger(), "%s: Goal is accepted",
     this->name().c_str());
-  this->_result_fut = this->_client->async_get_result(this->_goal_handle);
+  try
+  {
+    this->_result_fut = this->_client->async_get_result(this->_goal_handle);
+  }
+  catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e)
+  {
+    RCLCPP_ERROR(this->_node->get_logger(), "%s: %s",
+      this->name().c_str(), e.what());
+    return BT::NodeStatus::FAILURE;
+  }
   return BT::NodeStatus::RUNNING;
 }
 

From cb48a27c2569d8a7c1679f8036e735eb2981839e Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Thu, 9 Nov 2023 16:13:55 +0800
Subject: [PATCH 05/12] remove `value_or_abort()`, `value()` now returns ref

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 nexus_common/include/nexus_common/error.hpp   | 65 +++++++++----------
 nexus_common/src/error_test.cpp               |  8 +--
 .../src/job_manager_test.cpp                  | 12 ++--
 .../src/workcell_orchestrator.cpp             | 12 ++--
 4 files changed, 45 insertions(+), 52 deletions(-)

diff --git a/nexus_common/include/nexus_common/error.hpp b/nexus_common/include/nexus_common/error.hpp
index dd3d57e..cc47979 100644
--- a/nexus_common/include/nexus_common/error.hpp
+++ b/nexus_common/include/nexus_common/error.hpp
@@ -21,7 +21,6 @@
 #include <exception>
 #include <memory>
 #include <optional>
-#include <string>
 #include <type_traits>
 #include <variant>
 
@@ -53,41 +52,31 @@ public: template<typename D,
   Result(D d)
   : _var(std::make_shared<D>(std::move(d))) {}
 
-public: T* value()
+  /**
+   * Returns the value.
+   * @throws std::bad_variant_access if the result is an error.
+   */
+public: constexpr T& value()
   {
-    return std::get_if<T>(&this->_var);
+    return std::get<T>(this->_var);
   }
 
-public: const T* value() const
+  /**
+   * Returns the value.
+   * @throws std::bad_variant_access if the result is an error.
+   */
+public: constexpr const T& value() const
   {
-    return std::get_if<T>(&this->_var);
+    return std::get<T>(this->_var);
   }
 
-public: T* value_or_abort()
-  {
-    if (!this->value())
-    {
-      std::abort();
-    }
-    return this->value();
-  }
-
-public: const T* value_or_abort() const
-  {
-    if (!this->value())
-    {
-      std::abort();
-    }
-    return this->value();
-  }
-
-public: E* error()
+public: constexpr E* error()
   {
     auto* maybe_err = std::get_if<std::shared_ptr<E>>(&this->_var);
     return maybe_err ? maybe_err->get() : nullptr;
   }
 
-public: const E* error() const
+public: constexpr const E* error() const
   {
     auto* maybe_err = std::get_if<std::shared_ptr<E>>(&this->_var);
     return maybe_err ? maybe_err->get() : nullptr;
@@ -100,35 +89,39 @@ template<typename E>
 class Result<void, E>
 {
 public: Result()
-  : _var(std::nullopt) {}
+  : _err(std::nullopt) {}
 
 public: Result(std::shared_ptr<E> e)
-  : _var(std::move(e)) {}
+  : _err(std::move(e)) {}
 
 public: template<typename D,
     typename = std::enable_if_t<std::is_base_of_v<E, D>>>
   Result(D d)
-  : _var(std::make_shared<D>(std::move(d))) {}
+  : _err(std::make_shared<D>(std::move(d))) {}
 
-public: void value_or_abort() const
+  /**
+   * Throws if result is an error.
+   * @throws std::runtime_error if the result is an error.
+   */
+public: constexpr void value() const
   {
-    if (this->error())
+    if (this->_err.has_value())
     {
-      std::abort();
+      throw std::runtime_error("bad value access");
     }
   }
 
-public: E* error()
+public: constexpr E* error()
   {
-    return this->_var.has_value() ? this->_var->get() : nullptr;
+    return this->_err.has_value() ? this->_err->get() : nullptr;
   }
 
-public: const E* error() const
+public: constexpr const E* error() const
   {
-    return this->_var.has_value() ? this->_var->get() : nullptr;
+    return this->_err.has_value() ? this->_err->get() : nullptr;
   }
 
-private: std::optional<std::shared_ptr<E>> _var;
+private: std::optional<std::shared_ptr<E>> _err;
 };
 
 }
diff --git a/nexus_common/src/error_test.cpp b/nexus_common/src/error_test.cpp
index f915e08..11a0c4c 100644
--- a/nexus_common/src/error_test.cpp
+++ b/nexus_common/src/error_test.cpp
@@ -23,19 +23,19 @@ namespace nexus::common::test {
 
 TEST_CASE("get_result_value_and_error") {
   Result<int> good(1);
-  CHECK(*good.value() == 1);
+  CHECK(good.value() == 1);
   CHECK(!good.error());
 
   const Result<int> const_good(1);
-  CHECK(*const_good.value() == 1);
+  CHECK(const_good.value() == 1);
   CHECK(!const_good.error());
 
   Result<int> bad(std::make_shared<std::runtime_error>("bad"));
-  CHECK(!bad.value());
+  CHECK_THROWS(bad.value());
   CHECK(bad.error());
 
   Result<int> bad_val_constructor(std::runtime_error("bad_val_constructor"));
-  CHECK(!bad_val_constructor.value());
+  CHECK_THROWS(bad_val_constructor.value());
   CHECK(bad_val_constructor.error());
 
   Result<void> void_good;
diff --git a/nexus_workcell_orchestrator/src/job_manager_test.cpp b/nexus_workcell_orchestrator/src/job_manager_test.cpp
index 2db2e78..0cf8bd9 100644
--- a/nexus_workcell_orchestrator/src/job_manager_test.cpp
+++ b/nexus_workcell_orchestrator/src/job_manager_test.cpp
@@ -91,7 +91,7 @@ TEST_CASE("JobManager") {
 
   SECTION("assign task sets job data correctly") {
     Job* job;
-    job = *job_mgr.assign_task("test").value();
+    job = job_mgr.assign_task("test").value();
     CHECK(!job->bt.has_value());
     CHECK(!job->bt_logging);
     CHECK(job->ctx == nullptr);
@@ -102,7 +102,7 @@ TEST_CASE("JobManager") {
     CHECK(job->tick_count == 0);
 
     SECTION("ticking assigned but unqueued task is a noop") {
-      job_mgr.tick().value_or_abort();
+      job_mgr.tick().value();
       CHECK(job->tick_count == 0);
     }
 
@@ -110,7 +110,7 @@ TEST_CASE("JobManager") {
       handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle)
         {
           auto ctx = std::make_shared<Context>(fixture.node);
-          CHECK(*job_mgr.queue_task(goal_handle, ctx,
+          CHECK(job_mgr.queue_task(goal_handle, ctx,
             bt_factory.createTreeFromText(bt)).value() == job);
           CHECK(job->bt.has_value());
           CHECK(job->bt_logging);
@@ -141,7 +141,7 @@ TEST_CASE("JobManager") {
     std::vector<std::string> task_ids{"test1", "test2", "test3"};
     for (const auto& task_id : task_ids)
     {
-      job_mgr.assign_task(task_id).value_or_abort();
+      job_mgr.assign_task(task_id).value();
     }
 
     std::promise<void> done;
@@ -168,11 +168,11 @@ TEST_CASE("JobManager") {
     REQUIRE(done.get_future().wait_for(std::chrono::seconds(
         1)) == std::future_status::ready);
 
-    job_mgr.tick().value_or_abort();
+    job_mgr.tick().value();
     // test1 and test2 should be finished
     CHECK(job_mgr.jobs().size() == 1);
 
-    job_mgr.tick().value_or_abort();
+    job_mgr.tick().value();
     // test3 should now be finished
     CHECK(job_mgr.jobs().size() == 0);
   }
diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
index b588fe9..8b3a7a4 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
+++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
@@ -214,7 +214,7 @@ auto WorkcellOrchestrator::on_activate(
   RCLCPP_INFO(this->get_logger(), "Workcell activated");
   this->_bt_timer = this->create_wall_timer(BT_TICK_RATE, [this]()
       {
-        this->_job_mgr.value().tick().value_or_abort();
+        this->_job_mgr.value().tick().value();
       });
   return CallbackReturn::SUCCESS;
 }
@@ -224,7 +224,7 @@ auto WorkcellOrchestrator::on_deactivate(
 {
   RCLCPP_INFO(this->get_logger(),
     "Halting ongoing task before deactivating workcell");
-  this->_job_mgr->halt_all_jobs().value_or_abort();
+  this->_job_mgr->halt_all_jobs().value();
 
   this->_bt_timer->cancel();
   this->_bt_timer.reset();
@@ -245,7 +245,7 @@ auto WorkcellOrchestrator::on_cleanup(
   this->_signal_wc_srv.reset();
   this->_task_doable_srv.reset();
   this->_cmd_server.reset();
-  this->_job_mgr->halt_all_jobs().value_or_abort();
+  this->_job_mgr->halt_all_jobs().value();
   RCLCPP_INFO(this->get_logger(), "Cleaned up");
   return CallbackReturn::SUCCESS;
 }
@@ -301,7 +301,7 @@ auto WorkcellOrchestrator::_configure(
           "Cancelling specific task is no longer supported, all tasks will be cancelled together to ensure line consistency");
       }
 
-      this->_job_mgr->halt_all_jobs().value_or_abort();
+      this->_job_mgr->halt_all_jobs().value();
       return rclcpp_action::CancelResponse::ACCEPT;
     },
     [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<endpoints::WorkcellRequestAction::ActionType>>
@@ -327,7 +327,7 @@ auto WorkcellOrchestrator::_configure(
         goal_handle->abort(result);
         return;
       }
-      ctx->task = *task_result.value();
+      ctx->task = task_result.value();
 
       auto bt = this->_create_bt(ctx);
       auto job_result =
@@ -745,7 +745,7 @@ void WorkcellOrchestrator::_handle_task_doable(
       resp->success = false;
       return;
     }
-    resp->success = this->_can_perform_task(*r.value());
+    resp->success = this->_can_perform_task(r.value());
     if (resp->success)
     {
       RCLCPP_DEBUG(this->get_logger(), "Workcell can perform task");

From 0b3677cbb38d2c0ce03789e9d397d192fa0e1fcb Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Tue, 14 Nov 2023 16:09:05 +0800
Subject: [PATCH 06/12] remove exceptions

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 .../src/exceptions.hpp                        | 47 -------------------
 nexus_workcell_orchestrator/src/exit_code.hpp | 28 +++++++++++
 .../src/workcell_orchestrator.cpp             |  7 +--
 .../src/workcell_orchestrator.hpp             |  3 ++
 4 files changed, 35 insertions(+), 50 deletions(-)
 delete mode 100644 nexus_workcell_orchestrator/src/exceptions.hpp
 create mode 100644 nexus_workcell_orchestrator/src/exit_code.hpp

diff --git a/nexus_workcell_orchestrator/src/exceptions.hpp b/nexus_workcell_orchestrator/src/exceptions.hpp
deleted file mode 100644
index b9be0ed..0000000
--- a/nexus_workcell_orchestrator/src/exceptions.hpp
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * Copyright (C) 2022 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.
- *
- */
-
-#ifndef NEXUS_WORKCELL_ORCHESTRATOR__EXECPTIONS_HPP
-#define NEXUS_WORKCELL_ORCHESTRATOR__EXECPTIONS_HPP
-
-#include <stdexcept>
-#include <string>
-
-namespace nexus::workcell_orchestrator {
-
-class DiscoveryError : public std::runtime_error
-{
-public: DiscoveryError(const std::string& msg)
-  : std::runtime_error(msg) {}
-};
-
-class RegistrationError : public std::runtime_error
-{
-public: int32_t error_code;
-
-  /**
-   * Create new instance with message and error code.
-   * @param msg error message
-   * @param error_code error code as described in RegisterWorkcell or RegisterTransporter
-  */
-public: RegistrationError(const std::string& msg, int32_t error_code)
-  : std::runtime_error(msg), error_code(error_code) {}
-};
-
-}
-
-#endif
diff --git a/nexus_workcell_orchestrator/src/exit_code.hpp b/nexus_workcell_orchestrator/src/exit_code.hpp
new file mode 100644
index 0000000..459f86a
--- /dev/null
+++ b/nexus_workcell_orchestrator/src/exit_code.hpp
@@ -0,0 +1,28 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef NEXUS_WORKCELL_ORCHESTRATOR__EXIT_CODE_HPP
+#define NEXUS_WORKCELL_ORCHESTRATOR__EXIT_CODE_HPP
+
+namespace nexus::workcell_orchestrator {
+
+constexpr int EXIT_CODE_INVALID_PARAMETER = 1;
+constexpr int EXIT_CODE_REGISTRATION_FAILED = 2;
+
+}
+
+#endif
diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
index 8b3a7a4..5a97ee4 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
+++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
@@ -17,7 +17,7 @@
 
 #include "workcell_orchestrator.hpp"
 
-#include "exceptions.hpp"
+#include "exit_code.hpp"
 #include "get_joint_constraints.hpp"
 #include "get_result.hpp"
 #include "make_transform.hpp"
@@ -83,7 +83,8 @@ WorkcellOrchestrator::WorkcellOrchestrator(const rclcpp::NodeOptions& options)
     auto param = this->declare_parameter<std::string>("bt_path", "", desc);
     if (param.empty())
     {
-      throw std::runtime_error("param [bt_path] is required");
+      RCLCPP_FATAL(this->get_logger(), "param [bt_path] is required");
+      std::exit(EXIT_CODE_INVALID_PARAMETER);
     }
     this->_bt_path = std::move(param);
   }
@@ -661,7 +662,7 @@ void WorkcellOrchestrator::_register()
             RCLCPP_FATAL(this->get_logger(),
               "Failed to register with system orchestrator! [%s]",
               resp->message.c_str());
-            throw RegistrationError(resp->message, resp->error_code);
+            std::exit(EXIT_CODE_REGISTRATION_FAILED);
         }
         return;
       }
diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp
index 12b8717..3bdade0 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp
+++ b/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/error.hpp>
 
 #include <behaviortree_cpp_v3/action_node.h>
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -78,6 +79,8 @@ public: CallbackReturn on_deactivate(
 public: CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state)
   override;
 
+private: enum class ExiCode;
+
 private: rclcpp_action::Server<endpoints::WorkcellRequestAction::ActionType>::
   SharedPtr
     _cmd_server;

From 19c2cca3aa67012038bf12079d82be2397bded09 Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Tue, 14 Nov 2023 16:59:49 +0800
Subject: [PATCH 07/12] remove exceptions in system orchestrator

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 .../src/bid_transporter.cpp                   | 30 +++---------
 .../src/bid_transporter.hpp                   |  6 +--
 .../src/{exceptions.hpp => exit_code.hpp}     | 12 ++---
 .../src/system_orchestrator.cpp               | 48 +++++++++++--------
 .../src/system_orchestrator.hpp               |  3 +-
 5 files changed, 43 insertions(+), 56 deletions(-)
 rename nexus_system_orchestrator/src/{exceptions.hpp => exit_code.hpp} (73%)

diff --git a/nexus_system_orchestrator/src/bid_transporter.cpp b/nexus_system_orchestrator/src/bid_transporter.cpp
index af3dc25..7f5923e 100644
--- a/nexus_system_orchestrator/src/bid_transporter.cpp
+++ b/nexus_system_orchestrator/src/bid_transporter.cpp
@@ -27,20 +27,11 @@ BT::PortsList BidTransporter::providedPorts()
 
 BT::NodeStatus BidTransporter::onStart()
 {
-  auto node = this->_w_node.lock();
-  if (!node)
-  {
-    std::cerr <<
-      "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES" <<
-      std::endl;
-    std::terminate();
-  }
-
   auto maybe_destination = this->getInput<std::string>("destination");
   if (!maybe_destination)
   {
     RCLCPP_ERROR(
-      node->get_logger(), "%s: [destination] param is required",
+      this->_node->get_logger(), "%s: [destination] param is required",
       this->name().c_str());
     return BT::NodeStatus::FAILURE;
   }
@@ -49,7 +40,7 @@ BT::NodeStatus BidTransporter::onStart()
   auto req =
     std::make_shared<endpoints::IsTransporterAvailableService::ServiceType::Request>();
   req->request.id = this->_ctx->job_id;
-  req->request.requester = node->get_name();
+  req->request.requester = this->_node->get_name();
   req->request.destination = destination;
   // send request to all transporters in parallel
   for (auto& [transporter_id, session] : this->_ctx->transporter_sessions)
@@ -91,26 +82,17 @@ void BidTransporter::_cleanup_pending_requests()
 
 BT::NodeStatus BidTransporter::_update_ongoing_requests()
 {
-  auto node = this->_w_node.lock();
-  if (!node)
-  {
-    std::cerr <<
-      "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES" <<
-      std::endl;
-    std::terminate();
-  }
-
   if (std::chrono::steady_clock::now() > this->_time_limit)
   {
     for (const auto& [transporter_id, _] : this->_ongoing_requests)
     {
       RCLCPP_WARN(
-        node->get_logger(), "%s: Skipped transporter [%s] (no response)",
+        this->_node->get_logger(), "%s: Skipped transporter [%s] (no response)",
         this->name().c_str(),
         transporter_id.c_str());
     }
     RCLCPP_ERROR(
-      node->get_logger(), "%s: No transporter is able to perform task",
+      this->_node->get_logger(), "%s: No transporter is able to perform task",
       this->name().c_str());
     return BT::NodeStatus::FAILURE;
   }
@@ -128,7 +110,7 @@ BT::NodeStatus BidTransporter::_update_ongoing_requests()
       if (resp->available)
       {
         RCLCPP_INFO(
-          node->get_logger(), "[%s]: Bid awarded to [%s]",
+          this->_node->get_logger(), "[%s]: Bid awarded to [%s]",
           this->name().c_str(),
           transporter_id.c_str());
         this->setOutput("result", transporter_id);
@@ -138,7 +120,7 @@ BT::NodeStatus BidTransporter::_update_ongoing_requests()
       else
       {
         RCLCPP_DEBUG(
-          node->get_logger(), "%s: Transporter [%s] cannot perform task",
+          this->_node->get_logger(), "%s: Transporter [%s] cannot perform task",
           this->name().c_str(), transporter_id.c_str());
       }
     }
diff --git a/nexus_system_orchestrator/src/bid_transporter.hpp b/nexus_system_orchestrator/src/bid_transporter.hpp
index b0e0bd1..86123e3 100644
--- a/nexus_system_orchestrator/src/bid_transporter.hpp
+++ b/nexus_system_orchestrator/src/bid_transporter.hpp
@@ -56,9 +56,9 @@ public: static BT::PortsList providedPorts();
 
 public: BidTransporter(const std::string& name,
     const BT::NodeConfiguration& config,
-    rclcpp_lifecycle::LifecycleNode::WeakPtr w_node,
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<Context> ctx)
-  : BT::StatefulActionNode{name, config}, _w_node{w_node}, _ctx{std::move(ctx)}
+  : BT::StatefulActionNode{name, config}, _node{node}, _ctx{std::move(ctx)}
   {
   }
 
@@ -73,7 +73,7 @@ private: struct OngoingRequest
     FutureAndRequestId fut;
   };
 
-private: rclcpp_lifecycle::LifecycleNode::WeakPtr _w_node;
+private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
 private: std::shared_ptr<Context> _ctx;
 private: std::unordered_map<std::string, OngoingRequest> _ongoing_requests;
 private: std::chrono::steady_clock::time_point _time_limit;
diff --git a/nexus_system_orchestrator/src/exceptions.hpp b/nexus_system_orchestrator/src/exit_code.hpp
similarity index 73%
rename from nexus_system_orchestrator/src/exceptions.hpp
rename to nexus_system_orchestrator/src/exit_code.hpp
index 7d60191..2d33d94 100644
--- a/nexus_system_orchestrator/src/exceptions.hpp
+++ b/nexus_system_orchestrator/src/exit_code.hpp
@@ -15,18 +15,14 @@
  *
  */
 
-#ifndef NEXUS_SYSTEM_ORCHESTRATOR__EXCEPTIONS_HPP
-#define NEXUS_SYSTEM_ORCHESTRATOR__EXCEPTIONS_HPP
-
-#include <stdexcept>
+#ifndef NEXUS_SYSTEM_ORCHESTRATOR__EXIT_CODE_HPP
+#define NEXUS_SYSTEM_ORCHESTRATOR__EXIT_CODE_HPP
 
 namespace nexus::system_orchestrator {
 
-class DuplicatedWorkOrderError : public std::runtime_error
-{
-public: using std::runtime_error::runtime_error;
-};
+constexpr int EXIT_CODE_INVALID_PARAMETER = 1;
 
 }
 
+
 #endif
diff --git a/nexus_system_orchestrator/src/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp
index bd972e2..357ff5c 100644
--- a/nexus_system_orchestrator/src/system_orchestrator.cpp
+++ b/nexus_system_orchestrator/src/system_orchestrator.cpp
@@ -19,8 +19,8 @@
 
 #include "bid_transporter.hpp"
 #include "context.hpp"
-#include "exceptions.hpp"
 #include "execute_task.hpp"
+#include "exit_code.hpp"
 #include "for_each_task.hpp"
 #include "job.hpp"
 #include "send_signal.hpp"
@@ -53,6 +53,11 @@ using WorkcellState = endpoints::WorkcellStateTopic::MessageType;
 
 using rcl_interfaces::msg::ParameterDescriptor;
 
+class DuplicatedWorkOrderError : public std::runtime_error
+{
+public: using std::runtime_error::runtime_error;
+};
+
 /**
  * Error thrown when a work order cannot be completed.
  */
@@ -75,7 +80,8 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
     this->_bt_path = this->declare_parameter("bt_path", "", desc);
     if (this->_bt_path.empty())
     {
-      throw std::runtime_error("param [bt_path] is required");
+      RCLCPP_FATAL(this->get_logger(), "param [bt_path] is required");
+      std::exit(EXIT_CODE_INVALID_PARAMETER);
     }
 
     // check if "main.xml" exists
@@ -83,8 +89,10 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
     if (!std::filesystem::exists(main_bt) ||
       !std::filesystem::is_regular_file(main_bt))
     {
-      throw std::runtime_error(
-              "path specified in [bt_path] param must contain \"main.xml\"");
+      RCLCPP_FATAL(
+        this->get_logger(),
+        "path specified in [bt_path] param must contain \"main.xml\"");
+      std::exit(EXIT_CODE_INVALID_PARAMETER);
     }
   }
 
@@ -170,25 +178,23 @@ auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous)
     std::shared_ptr<const WorkOrderActionType::Goal> goal)
     -> rclcpp_action::GoalResponse
     {
-      try
+      if (this->_max_parallel_jobs > 0 &&
+      this->_jobs.size() >= static_cast<size_t>(this->_max_parallel_jobs))
       {
-        if (this->_max_parallel_jobs > 0 &&
-        this->_jobs.size() >= static_cast<size_t>(this->_max_parallel_jobs))
-        {
-          auto result = std::make_shared<WorkOrderActionType::Result>();
-          result->message = "Max number of parallel work orders reached";
-          RCLCPP_ERROR(this->get_logger(), "%s", result->message.c_str());
-          return rclcpp_action::GoalResponse::REJECT;
-        }
-
-        this->_create_job(*goal);
-        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+        auto result = std::make_shared<WorkOrderActionType::Result>();
+        result->message = "Max number of parallel work orders reached";
+        RCLCPP_ERROR(this->get_logger(), "%s", result->message.c_str());
+        return rclcpp_action::GoalResponse::REJECT;
       }
-      catch (const std::exception& e)
+
+      const auto r = this->_create_job(*goal);
+      if (r.error())
       {
-        RCLCPP_ERROR(this->get_logger(), "Failed to create job: %s", e.what());
+        RCLCPP_ERROR(this->get_logger(), "Failed to create job: %s",
+        r.error()->what());
         return rclcpp_action::GoalResponse::REJECT;
       }
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     },
     [this](const std::shared_ptr<WorkOrderGoalHandle> goal_handle)
     -> rclcpp_action::CancelResponse
@@ -508,7 +514,8 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo,
   return bt_factory->createTreeFromFile(this->_bt_path / "main.xml");
 }
 
-void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal)
+common::Result<void> SystemOrchestrator::_create_job(
+  const WorkOrderActionType::Goal& goal)
 {
   auto wo =
     YAML::Load(goal.order.work_order).as<common::WorkOrder>();
@@ -530,8 +537,9 @@ void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal)
     auto result = std::make_shared<WorkOrderActionType::Result>();
     result->message = "A work order with the same id is already running!";
     RCLCPP_ERROR(this->get_logger(), "%s", result->message.c_str());
-    throw DuplicatedWorkOrderError{result->message};
+    return DuplicatedWorkOrderError(result->message);
   }
+  return common::Result<void>();
 }
 
 void SystemOrchestrator::_init_job(
diff --git a/nexus_system_orchestrator/src/system_orchestrator.hpp b/nexus_system_orchestrator/src/system_orchestrator.hpp
index 0cbc8a3..3621bdb 100644
--- a/nexus_system_orchestrator/src/system_orchestrator.hpp
+++ b/nexus_system_orchestrator/src/system_orchestrator.hpp
@@ -22,6 +22,7 @@
 #include "session.hpp"
 
 #include <nexus_common/action_client_bt_node.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_common/models/work_order.hpp>
 #include <nexus_common/sync_service_client.hpp>
 #include <nexus_endpoints.hpp>
@@ -110,7 +111,7 @@ class SystemOrchestrator : public
    * required contexts and start the job.
    * @throws std::exception
    */
-  void _create_job(const WorkOrderActionType::Goal& goal);
+  [[nodiscard]] common::Result<void> _create_job(const WorkOrderActionType::Goal& goal);
 
   /**
    * Assigns all tasks and start the job associated with the goal handle.

From c0206f66d50b8e86fe164f9079f41b5fb02a22fe Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Mon, 20 Nov 2023 10:54:39 +0800
Subject: [PATCH 08/12] styling

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 nexus_system_orchestrator/src/system_orchestrator.hpp | 3 ++-
 nexus_workcell_orchestrator/src/job_manager.hpp       | 3 ++-
 nexus_workcell_orchestrator/src/task_parser.hpp       | 3 ++-
 3 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/nexus_system_orchestrator/src/system_orchestrator.hpp b/nexus_system_orchestrator/src/system_orchestrator.hpp
index 3621bdb..4cd8777 100644
--- a/nexus_system_orchestrator/src/system_orchestrator.hpp
+++ b/nexus_system_orchestrator/src/system_orchestrator.hpp
@@ -111,7 +111,8 @@ class SystemOrchestrator : public
    * required contexts and start the job.
    * @throws std::exception
    */
-  [[nodiscard]] common::Result<void> _create_job(const WorkOrderActionType::Goal& goal);
+  [[nodiscard]] common::Result<void> _create_job(
+    const WorkOrderActionType::Goal& goal);
 
   /**
    * Assigns all tasks and start the job associated with the goal handle.
diff --git a/nexus_workcell_orchestrator/src/job_manager.hpp b/nexus_workcell_orchestrator/src/job_manager.hpp
index f9a7fd2..a2425f1 100644
--- a/nexus_workcell_orchestrator/src/job_manager.hpp
+++ b/nexus_workcell_orchestrator/src/job_manager.hpp
@@ -69,7 +69,8 @@ public: [[nodiscard]] common::Result<Job*> queue_task(
   /**
    * Remove a assigned task.
    */
-public: [[nodiscard]] common::Result<void> remove_assigned_task(const std::string& task_id);
+public: [[nodiscard]] common::Result<void> remove_assigned_task(
+    const std::string& task_id);
 
   /**
    * Forcefully stop and clear all jobs. Jobs will be stopped as soon as possible, unlike
diff --git a/nexus_workcell_orchestrator/src/task_parser.hpp b/nexus_workcell_orchestrator/src/task_parser.hpp
index e914ba7..b473a2f 100644
--- a/nexus_workcell_orchestrator/src/task_parser.hpp
+++ b/nexus_workcell_orchestrator/src/task_parser.hpp
@@ -36,7 +36,8 @@ public: TaskParser() {}
   /**
    * Parses a workcell task.
    */
-public: common::Result<Task> parse_task(const nexus_orchestrator_msgs::msg::WorkcellTask& task);
+public: common::Result<Task> parse_task(
+    const nexus_orchestrator_msgs::msg::WorkcellTask& task);
 
   /**
    * Add task type to remap to.

From 13e02aadad6142afe6c017d68cf910e3f1b1d782 Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Mon, 20 Nov 2023 13:17:14 +0800
Subject: [PATCH 09/12] remove exception from capabilities

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 nexus_capabilities/CMakeLists.txt             |  1 +
 .../include/nexus_capabilities/capability.hpp |  3 +-
 .../include/nexus_capabilities/context.hpp    | 19 +++++++-
 .../include/nexus_capabilities/exceptions.hpp | 37 ---------------
 .../src/capabilities/detection_capability.cpp | 24 +++-------
 .../src/capabilities/detection_capability.hpp |  3 +-
 .../capabilities/dispense_item_capability.cpp |  6 +--
 .../capabilities/dispense_item_capability.hpp |  4 +-
 .../src/capabilities/execute_trajectory.hpp   |  9 ++--
 .../execute_trajectory_capability.cpp         | 14 ++----
 .../execute_trajectory_capability.hpp         |  4 +-
 .../src/capabilities/gripper_capability.cpp   |  4 +-
 .../src/capabilities/gripper_capability.hpp   |  3 +-
 .../capabilities/plan_motion_capability.cpp   |  5 +-
 .../capabilities/plan_motion_capability.hpp   |  4 +-
 nexus_capabilities/src/context.cpp            | 46 +++++++++++++++++++
 .../src/job_manager_test.cpp                  |  4 +-
 .../src/task_results_test.cpp                 |  2 +-
 .../src/workcell_orchestrator.cpp             | 34 +++++++-------
 19 files changed, 121 insertions(+), 105 deletions(-)
 delete mode 100644 nexus_capabilities/include/nexus_capabilities/exceptions.hpp
 create mode 100644 nexus_capabilities/src/context.cpp

diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt
index 6f9d4a3..c9e590e 100644
--- a/nexus_capabilities/CMakeLists.txt
+++ b/nexus_capabilities/CMakeLists.txt
@@ -56,6 +56,7 @@ include(GNUInstallDirs)
 # nexus_capabilities library
 
 add_library(${PROJECT_NAME} SHARED
+  src/context.cpp
   src/conversions/pose_stamped.cpp
 )
 
diff --git a/nexus_capabilities/include/nexus_capabilities/capability.hpp b/nexus_capabilities/include/nexus_capabilities/capability.hpp
index 830febf..5469dc4 100644
--- a/nexus_capabilities/include/nexus_capabilities/capability.hpp
+++ b/nexus_capabilities/include/nexus_capabilities/capability.hpp
@@ -22,6 +22,7 @@
 #include "task.hpp"
 
 #include <nexus_common/bt_store.hpp>
+#include <nexus_common/error.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
@@ -54,7 +55,7 @@ class Capability
    * @param bt_factory Must be valid for the lifetime of the capability.
    * @throws StateTransitionError if transition fails.
    */
-  virtual void configure(
+  [[nodiscard]] virtual common::Result<void> configure(
     rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) = 0;
diff --git a/nexus_capabilities/include/nexus_capabilities/context.hpp b/nexus_capabilities/include/nexus_capabilities/context.hpp
index 75d5cfa..a8c1011 100644
--- a/nexus_capabilities/include/nexus_capabilities/context.hpp
+++ b/nexus_capabilities/include/nexus_capabilities/context.hpp
@@ -20,6 +20,11 @@
 
 #include "task.hpp"
 
+#include <nexus_common/error.hpp>
+#include <nexus_endpoints.hpp>
+#include <nexus_orchestrator_msgs/msg/task_progress.hpp>
+
+#include <rclcpp_action/rclcpp_action.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 
 #include <memory>
@@ -32,6 +37,9 @@ namespace nexus {
 
 class Context
 {
+public: using GoalHandlePtr =
+    std::shared_ptr<rclcpp_action::ServerGoalHandle<endpoints::WorkcellRequestAction::ActionType>>;
+
   /**
    * There are several choices we can use here, each having their own pros and cons.
    *
@@ -65,8 +73,15 @@ public: Task task;
 public: std::vector<std::string> errors;
 public: std::unordered_set<std::string> signals;
 
-public: Context(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
-  : node(std::move(node)) {}
+public: Context(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node,
+    GoalHandlePtr goal_handle)
+  : node(std::move(node)), _goal_handle(std::move(goal_handle)) {}
+
+public: common::Result<void> publish_feedback(
+    const nexus_orchestrator_msgs::msg::TaskProgress& progress,
+    const std::string& msg = "");
+
+private: GoalHandlePtr _goal_handle;
 };
 
 } // namespace nexus
diff --git a/nexus_capabilities/include/nexus_capabilities/exceptions.hpp b/nexus_capabilities/include/nexus_capabilities/exceptions.hpp
deleted file mode 100644
index 64100f6..0000000
--- a/nexus_capabilities/include/nexus_capabilities/exceptions.hpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * Copyright (C) 2022 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.
- *
- */
-
-#ifndef NEXUS_CAPABILITIES__EXCEPTIONS_HPP
-#define NEXUS_CAPABILITIES__EXCEPTIONS_HPP
-
-#include <stdexcept>
-#include <string>
-
-namespace nexus {
-
-/**
- * Base class for all errors from capabilities.
- */
-class CapabilityError : public std::runtime_error
-{
-public: CapabilityError(const std::string& msg)
-  : std::runtime_error(msg) {}
-};
-
-}
-
-#endif // NEXUS_CAPABILITIES__EXCEPTIONS_HPP
diff --git a/nexus_capabilities/src/capabilities/detection_capability.cpp b/nexus_capabilities/src/capabilities/detection_capability.cpp
index c546411..05975f3 100644
--- a/nexus_capabilities/src/capabilities/detection_capability.cpp
+++ b/nexus_capabilities/src/capabilities/detection_capability.cpp
@@ -37,7 +37,7 @@ void DetectionCapability::declare_params(
 }
 
 //==============================================================================
-void DetectionCapability::configure(
+common::Result<void> DetectionCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> ctx_mgr,
   BT::BehaviorTreeFactory& bt_factory)
@@ -57,18 +57,11 @@ void DetectionCapability::configure(
 
   bt_factory.registerBuilder<DetectOffset>(
     "detection.DetectOffset",
-    [this, ctx_mgr, w_node = std::weak_ptr{node}](const std::string& name,
+    [this, ctx_mgr, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
-      auto node = w_node.lock();
-      if (!node)
-      {
-        std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl;
-        std::terminate();
-      }
-
       return std::make_unique<DetectOffset>(name, config,
-      node->get_logger(), ctx_mgr, [this, node](const std::string& detector)
+      node->get_logger(), ctx_mgr, [this](const std::string& detector)
       {
         return this->_clients.at(detector);
       });
@@ -76,16 +69,9 @@ void DetectionCapability::configure(
 
   bt_factory.registerBuilder<DetectAllItems>(
     "detection.DetectAllItems",
-    [this, ctx_mgr, w_node = std::weak_ptr{node}](const std::string& name,
+    [this, ctx_mgr, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
-      auto node = w_node.lock();
-      if (!node)
-      {
-        std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl;
-        std::terminate();
-      }
-
       return std::make_unique<DetectAllItems>(name, config, node, ctx_mgr,
       [this](const std::string& detector)
       {
@@ -106,6 +92,8 @@ void DetectionCapability::configure(
     {
       return std::make_unique<GetDetectionPose>(name, config, ctx_mgr);
     });
+
+  return common::Result<void>();
 }
 
 //==============================================================================
diff --git a/nexus_capabilities/src/capabilities/detection_capability.hpp b/nexus_capabilities/src/capabilities/detection_capability.hpp
index ced80a5..81c72c1 100644
--- a/nexus_capabilities/src/capabilities/detection_capability.hpp
+++ b/nexus_capabilities/src/capabilities/detection_capability.hpp
@@ -20,6 +20,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -45,7 +46,7 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& node) final;
   /**
    * @copydoc Capability::configure
    */
-public: void configure(
+public: common::Result<void> configure(
     rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
diff --git a/nexus_capabilities/src/capabilities/dispense_item_capability.cpp b/nexus_capabilities/src/capabilities/dispense_item_capability.cpp
index 1277d8c..9288d3c 100644
--- a/nexus_capabilities/src/capabilities/dispense_item_capability.cpp
+++ b/nexus_capabilities/src/capabilities/dispense_item_capability.cpp
@@ -19,8 +19,6 @@
 
 #include "dispense_item_task_data.hpp"
 
-#include <nexus_capabilities/exceptions.hpp>
-
 #include <yaml-cpp/exceptions.h>
 
 namespace nexus::capabilities {
@@ -47,7 +45,7 @@ void DispenseItemCapability::declare_params(
 }
 
 //==============================================================================
-void DispenseItemCapability::configure(
+common::Result<void> DispenseItemCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> /* ctx_mgr */,
   BT::BehaviorTreeFactory& bt_factory)
@@ -76,6 +74,8 @@ void DispenseItemCapability::configure(
       return std::make_unique<DispenseItem>(name, config,
       node, this->_dispensers, std::chrono::milliseconds{dispenser_timeout});
     });
+
+  return common::Result<void>();
 }
 
 }
diff --git a/nexus_capabilities/src/capabilities/dispense_item_capability.hpp b/nexus_capabilities/src/capabilities/dispense_item_capability.hpp
index 15b5923..b01faaf 100644
--- a/nexus_capabilities/src/capabilities/dispense_item_capability.hpp
+++ b/nexus_capabilities/src/capabilities/dispense_item_capability.hpp
@@ -22,6 +22,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -48,7 +49,8 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& node) final;
   /**
    * @copydoc Capability::configure
    */
-public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
 
diff --git a/nexus_capabilities/src/capabilities/execute_trajectory.hpp b/nexus_capabilities/src/capabilities/execute_trajectory.hpp
index 768717e..3210187 100644
--- a/nexus_capabilities/src/capabilities/execute_trajectory.hpp
+++ b/nexus_capabilities/src/capabilities/execute_trajectory.hpp
@@ -35,7 +35,7 @@ namespace nexus::capabilities {
  *   trajectory |moveit_msgs::msg::RobotTrajectory| The trajectory to execute.
  */
 class ExecuteTrajectory : public common
-  ::ActionClientBtNode<rclcpp_lifecycle::LifecycleNode*,
+  ::ActionClientBtNode<rclcpp_lifecycle::LifecycleNode::SharedPtr,
     endpoints::ControllerRobotTrajectoryAction::ActionType>
 {
 public: using ActionType =
@@ -54,9 +54,10 @@ public: static BT::PortsList providedPorts();
    */
 public: inline ExecuteTrajectory(const std::string& name,
     const BT::NodeConfiguration& config,
-    rclcpp_lifecycle::LifecycleNode& node)
-  : common::ActionClientBtNode<rclcpp_lifecycle::LifecycleNode*, ActionType>(
-      name, config, &node) {}
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+  : common::ActionClientBtNode<rclcpp_lifecycle::LifecycleNode::SharedPtr,
+      ActionType>(
+      name, config, std::move(node)) {}
 
 protected: std::string get_action_name() const
   override
diff --git a/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp b/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp
index 8d66b25..d04e0ec 100644
--- a/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp
+++ b/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp
@@ -25,25 +25,19 @@
 
 namespace nexus::capabilities {
 
-void ExecuteTrajectoryCapability::configure(
+common::Result<void> ExecuteTrajectoryCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> /* ctx_mgr */,
   BT::BehaviorTreeFactory& bt_factory)
 {
   bt_factory.registerBuilder<ExecuteTrajectory>(
     "execute_trajectory.ExecuteTrajectory",
-    [w_node = std::weak_ptr{node}](const std::string& name,
+    [node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
-      auto node = w_node.lock();
-      if (!node)
-      {
-        std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl;
-        std::terminate();
-      }
-
-      return std::make_unique<ExecuteTrajectory>(name, config, *node);
+      return std::make_unique<ExecuteTrajectory>(name, config, node);
     });
+  return common::Result<void>();
 }
 
 }
diff --git a/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp b/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp
index d544f5e..d818712 100644
--- a/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp
+++ b/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp
@@ -22,6 +22,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -48,7 +49,8 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& /* node */) final
   /**
    * @copydoc Capability::configure
    */
-public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
 
diff --git a/nexus_capabilities/src/capabilities/gripper_capability.cpp b/nexus_capabilities/src/capabilities/gripper_capability.cpp
index b4ae30d..3a55abf 100644
--- a/nexus_capabilities/src/capabilities/gripper_capability.cpp
+++ b/nexus_capabilities/src/capabilities/gripper_capability.cpp
@@ -31,7 +31,7 @@ void GripperCapability::declare_params(rclcpp_lifecycle::LifecycleNode& node)
   node.declare_parameter("grippers", std::vector<std::string>{}, desc);
 }
 
-void GripperCapability::configure(
+common::Result<void> GripperCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> /* ctx_mgr */,
   BT::BehaviorTreeFactory& bt_factory)
@@ -48,6 +48,8 @@ void GripperCapability::configure(
     {
       return std::make_unique<GripperControl>(name, config, *node);
     });
+
+  return common::Result<void>();
 }
 
 }
diff --git a/nexus_capabilities/src/capabilities/gripper_capability.hpp b/nexus_capabilities/src/capabilities/gripper_capability.hpp
index 82e541a..09b901e 100644
--- a/nexus_capabilities/src/capabilities/gripper_capability.hpp
+++ b/nexus_capabilities/src/capabilities/gripper_capability.hpp
@@ -20,6 +20,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -45,7 +46,7 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& node) final;
   /**
    * @copydoc Capability::configure
    */
-public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
 
diff --git a/nexus_capabilities/src/capabilities/plan_motion_capability.cpp b/nexus_capabilities/src/capabilities/plan_motion_capability.cpp
index 7745047..da3e1e1 100644
--- a/nexus_capabilities/src/capabilities/plan_motion_capability.cpp
+++ b/nexus_capabilities/src/capabilities/plan_motion_capability.cpp
@@ -19,15 +19,13 @@
 
 #include "plan_motion.hpp"
 
-#include <nexus_capabilities/exceptions.hpp>
-
 #include <tf2_ros/transform_broadcaster.h>
 
 #include <memory>
 
 namespace nexus::capabilities {
 
-void PlanMotionCapability::configure(
+common::Result<void> PlanMotionCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> /* ctx_mgr */,
   BT::BehaviorTreeFactory& bt_factory)
@@ -43,6 +41,7 @@ void PlanMotionCapability::configure(
       return std::make_unique<PlanMotion>(name, config, *node, this->_client,
       tf_broadcaster);
     });
+  return common::Result<void>();
 }
 
 }
diff --git a/nexus_capabilities/src/capabilities/plan_motion_capability.hpp b/nexus_capabilities/src/capabilities/plan_motion_capability.hpp
index 2622965..f2c7a2b 100644
--- a/nexus_capabilities/src/capabilities/plan_motion_capability.hpp
+++ b/nexus_capabilities/src/capabilities/plan_motion_capability.hpp
@@ -20,6 +20,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -45,7 +46,8 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& /* node */) final
   /**
    * @copydoc Capability::configure
    */
-public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
 
diff --git a/nexus_capabilities/src/context.cpp b/nexus_capabilities/src/context.cpp
new file mode 100644
index 0000000..d2d5f36
--- /dev/null
+++ b/nexus_capabilities/src/context.cpp
@@ -0,0 +1,46 @@
+/*
+ * 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.
+ *
+ */
+
+#include "nexus_capabilities/context.hpp"
+
+#include <exception>
+
+namespace nexus {
+
+common::Result<void> Context::publish_feedback(
+  const nexus_orchestrator_msgs::msg::TaskProgress& progress,
+  const std::string& msg)
+{
+  endpoints::WorkcellRequestAction::ActionType::Feedback::SharedPtr feedback_msg;
+  feedback_msg->state.task_id = this->task.id;
+  feedback_msg->state.workcell_id = this->node->get_name();
+  feedback_msg->state.status =
+    nexus_orchestrator_msgs::msg::TaskState::STATUS_RUNNING;
+  feedback_msg->state.progress = progress;
+  feedback_msg->state.message = msg;
+  try
+  {
+    this->_goal_handle->publish_feedback(std::move(feedback_msg));
+    return common::Result<void>();
+  }
+  catch (const std::runtime_error& e)
+  {
+    return e;
+  }
+}
+
+}
diff --git a/nexus_workcell_orchestrator/src/job_manager_test.cpp b/nexus_workcell_orchestrator/src/job_manager_test.cpp
index 0cf8bd9..04179a1 100644
--- a/nexus_workcell_orchestrator/src/job_manager_test.cpp
+++ b/nexus_workcell_orchestrator/src/job_manager_test.cpp
@@ -109,7 +109,7 @@ TEST_CASE("JobManager") {
     SECTION("can queue an assigned task") {
       handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle)
         {
-          auto ctx = std::make_shared<Context>(fixture.node);
+          auto ctx = std::make_shared<Context>(fixture.node, goal_handle);
           CHECK(job_mgr.queue_task(goal_handle, ctx,
             bt_factory.createTreeFromText(bt)).value() == job);
           CHECK(job->bt.has_value());
@@ -148,7 +148,7 @@ TEST_CASE("JobManager") {
     size_t queued = 0;
     handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle)
       {
-        auto ctx = std::make_shared<Context>(fixture.node);
+        auto ctx = std::make_shared<Context>(fixture.node, goal_handle);
         REQUIRE(job_mgr.queue_task(goal_handle, ctx,
           bt_factory.createTreeFromText(bt)).value());
         if (++queued == task_ids.size())
diff --git a/nexus_workcell_orchestrator/src/task_results_test.cpp b/nexus_workcell_orchestrator/src/task_results_test.cpp
index 21b2b37..51503a8 100644
--- a/nexus_workcell_orchestrator/src/task_results_test.cpp
+++ b/nexus_workcell_orchestrator/src/task_results_test.cpp
@@ -34,7 +34,7 @@ namespace nexus::workcell_orchestrator::test {
 TEST_CASE("get and set results") {
   auto fixture = common::test::RosFixture<rclcpp_lifecycle::LifecycleNode>{};
   auto ctx_mgr = std::make_shared<ContextManager>();
-  auto ctx = std::make_shared<Context>(fixture.node);
+  auto ctx = std::make_shared<Context>(fixture.node, nullptr);
   ctx_mgr->set_active_context(ctx);
   BT::BehaviorTreeFactory bt_factory;
 
diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
index 5a97ee4..f664ff2 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
+++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
@@ -317,7 +317,7 @@ auto WorkcellOrchestrator::_configure(
         return;
       }
 
-      auto ctx = std::make_shared<Context>(this->shared_from_this());
+      auto ctx = std::make_shared<Context>(this->shared_from_this(), goal_handle);
       auto task_result =
       this->_task_parser.parse_task(goal_handle->get_goal()->task);
       if (task_result.error())
@@ -564,30 +564,28 @@ auto WorkcellOrchestrator::_configure(
   }
 
   // configure capabilities
-  try
+  RCLCPP_INFO(
+    this->get_logger(),
+    "Configuring generic capabilities..."
+  );
+  for (auto& [cap_id, cap] : this->_capabilities)
   {
     RCLCPP_INFO(
       this->get_logger(),
-      "Configuring generic capabilities..."
+      "configuring capability [%s]",
+      cap_id.c_str()
     );
-    for (auto& [cap_id, cap] : this->_capabilities)
+    const auto r = cap->configure(
+      this->shared_from_this(), this->_job_mgr->context_manager(),
+      *_bt_factory);
+    if (r.error())
     {
-      RCLCPP_INFO(
-        this->get_logger(),
-        "configuring capability [%s]",
-        cap_id.c_str()
-      );
-      cap->configure(
-        this->shared_from_this(), this->_job_mgr->context_manager(),
-        *_bt_factory);
+      RCLCPP_ERROR(
+        this->get_logger(), "Failed to configure capability (%s)",
+        r.error()->what());
+      return CallbackReturn::FAILURE;
     }
   }
-  catch (const CapabilityError& e)
-  {
-    RCLCPP_ERROR(
-      this->get_logger(), "Failed to configure capability (%s)", e.what());
-    return CallbackReturn::FAILURE;
-  }
 
   // Create map for remapping capabilities
   auto remap_caps = this->get_parameter("remap_task_types").as_string();

From b5606206352e1396d9b2cc8c5d4945f8b2a9931c Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Mon, 20 Nov 2023 13:22:30 +0800
Subject: [PATCH 10/12] remove deleted header

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 nexus_workcell_orchestrator/src/workcell_orchestrator.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
index f664ff2..15327e5 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
+++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
@@ -27,7 +27,6 @@
 #include "transform_pose.hpp"
 
 #include <nexus_capabilities/context.hpp>
-#include <nexus_capabilities/exceptions.hpp>
 #include <nexus_capabilities/utils.hpp>
 #include <nexus_common/logging.hpp>
 #include <nexus_common/pausable_sequence.hpp>

From 7e83337fba0b6c48acfd4ac08091adc30886bbc3 Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Tue, 21 Nov 2023 16:53:25 +0800
Subject: [PATCH 11/12] style

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 nexus_capabilities/src/capabilities/gripper_capability.hpp | 3 ++-
 nexus_workcell_orchestrator/src/workcell_orchestrator.cpp  | 3 ++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/nexus_capabilities/src/capabilities/gripper_capability.hpp b/nexus_capabilities/src/capabilities/gripper_capability.hpp
index 09b901e..02fe6be 100644
--- a/nexus_capabilities/src/capabilities/gripper_capability.hpp
+++ b/nexus_capabilities/src/capabilities/gripper_capability.hpp
@@ -46,7 +46,8 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& node) final;
   /**
    * @copydoc Capability::configure
    */
-public: common::Result<void> configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
 
diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
index 15327e5..5092968 100644
--- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
+++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp
@@ -316,7 +316,8 @@ auto WorkcellOrchestrator::_configure(
         return;
       }
 
-      auto ctx = std::make_shared<Context>(this->shared_from_this(), goal_handle);
+      auto ctx = std::make_shared<Context>(
+        this->shared_from_this(), goal_handle);
       auto task_result =
       this->_task_parser.parse_task(goal_handle->get_goal()->task);
       if (task_result.error())

From b85cc406cedfef24b34c7c6e2bd562ced04739ad Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 27 Nov 2024 12:44:34 +0800
Subject: [PATCH 12/12] Include correct header

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 nexus_common/src/error_test.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nexus_common/src/error_test.cpp b/nexus_common/src/error_test.cpp
index 11a0c4c..4eba198 100644
--- a/nexus_common/src/error_test.cpp
+++ b/nexus_common/src/error_test.cpp
@@ -15,7 +15,7 @@
  *
  */
 
-#include <catch2/catch.hpp>
+#include <rmf_utils/catch.hpp>
 
 #include "nexus_common/error.hpp"