From 8c09dbf1b174bf70c7aecd497ec3bf6c4a120037 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 7 Jan 2025 15:39:59 +0800
Subject: [PATCH] Skeleton of endpoint and service

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 nexus_endpoints.redf.yaml                     |  7 ++++++-
 nexus_endpoints/nexus_endpoints.hpp           | 21 +++++++++++++++++++
 .../nexus_orchestrator_msgs/CMakeLists.txt    |  1 +
 .../srv/UnregisterWorkcell.srv                | 19 +++++++++++++++++
 4 files changed, 47 insertions(+), 1 deletion(-)
 create mode 100644 nexus_msgs/nexus_orchestrator_msgs/srv/UnregisterWorkcell.srv

diff --git a/nexus_endpoints.redf.yaml b/nexus_endpoints.redf.yaml
index d8a20a1c..e8152f14 100644
--- a/nexus_endpoints.redf.yaml
+++ b/nexus_endpoints.redf.yaml
@@ -59,8 +59,13 @@ endpoints:
     description: Register a workcell
     type: service
     service_name: /register_workcell
-  # Robot Arm Controller
     service_type: nexus_orchestrator_msgs/srv/RegisterWorkcell
+  - title: Unregister Workcell Service
+    description: Unregister a workcell
+    type: service
+    service_name: /unregister_workcell
+    service_type: nexus_orchestrator_msgs/srv/UnregisterWorkcell
+  # Robot Arm Controller
   - title: Controller Robot Trajectory Action
     description: Send a controller action goal to a robot arm
     type: action
diff --git a/nexus_endpoints/nexus_endpoints.hpp b/nexus_endpoints/nexus_endpoints.hpp
index b4121385..5c72fccd 100644
--- a/nexus_endpoints/nexus_endpoints.hpp
+++ b/nexus_endpoints/nexus_endpoints.hpp
@@ -24,6 +24,7 @@
 #include <nexus_orchestrator_msgs/srv/register_workcell.hpp>
 #include <nexus_orchestrator_msgs/srv/remove_pending_task.hpp>
 #include <nexus_orchestrator_msgs/srv/signal_workcell.hpp>
+#include <nexus_orchestrator_msgs/srv/unregister_workcell.hpp>
 #include <nexus_transporter_msgs/action/transport.hpp>
 #include <nexus_transporter_msgs/srv/is_transporter_available.hpp>
 #include <trajectory_msgs/msg/joint_trajectory.hpp>
@@ -255,6 +256,26 @@ class RegisterWorkcellService {
   }
 };
 
+class UnregisterWorkcellService {
+public:
+  using ServiceType = nexus_orchestrator_msgs::srv::UnregisterWorkcell;
+
+  static inline std::string service_name() {
+    const std::string name = "/unregister_workcell";
+    return name;
+  }
+
+  template<typename NodePtrT, typename CallbackT>
+  static rclcpp::Service<UnregisterWorkcellService::ServiceType>::SharedPtr create_service(NodePtrT node, CallbackT&& callback) {
+    return node->template create_service<UnregisterWorkcellService::ServiceType>(UnregisterWorkcellService::service_name(), std::forward<CallbackT>(callback));
+  }
+
+  template<typename NodePtrT>
+  static rclcpp::Client<UnregisterWorkcellService::ServiceType>::SharedPtr create_client(NodePtrT node) {
+    return node->template create_client<UnregisterWorkcellService::ServiceType>(UnregisterWorkcellService::service_name());
+  }
+};
+
 class ControllerRobotTrajectoryAction {
 public:
   using ActionType = control_msgs::action::FollowJointTrajectory;
diff --git a/nexus_msgs/nexus_orchestrator_msgs/CMakeLists.txt b/nexus_msgs/nexus_orchestrator_msgs/CMakeLists.txt
index 9ab5787a..63032da4 100644
--- a/nexus_msgs/nexus_orchestrator_msgs/CMakeLists.txt
+++ b/nexus_msgs/nexus_orchestrator_msgs/CMakeLists.txt
@@ -39,6 +39,7 @@ set(srv_files
   "srv/RegisterWorkcell.srv"
   "srv/RemovePendingTask.srv"
   "srv/SignalWorkcell.srv"
+  "srv/UnregisterWorkcell.srv"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}
diff --git a/nexus_msgs/nexus_orchestrator_msgs/srv/UnregisterWorkcell.srv b/nexus_msgs/nexus_orchestrator_msgs/srv/UnregisterWorkcell.srv
new file mode 100644
index 00000000..1bab57dd
--- /dev/null
+++ b/nexus_msgs/nexus_orchestrator_msgs/srv/UnregisterWorkcell.srv
@@ -0,0 +1,19 @@
+# Service to unregister a workcell with the system orchestrator
+
+# Description provided by the workcell for unregistration
+WorkcellDescription description
+
+---
+
+# True if workcell is removed successfully
+bool success
+
+int32 error_code
+int32 ERROR_UNKNOWN=0
+# System orchestrator is not ready
+int32 ERROR_NOT_READY=1
+# The workcell was not registered
+int32 ERROR_NOT_REGISTERED=2
+
+# [OPTIONAL] message for debugging or indicating cause of success/failure
+string message