From 460726596f0ea0ec598474980d9d1ee7fa310caa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Miguel=20=C3=81ngel=20Gonz=C3=A1lez=20Santamarta?=
 <mgons@unileon.es>
Date: Sat, 8 Jun 2024 18:20:22 +0200
Subject: [PATCH] wait_for_service timeout added to C++

---
 .../include/yasmin_ros/basic_outcomes.hpp     |  1 +
 .../include/yasmin_ros/service_state.hpp      | 29 ++++++++++++++-----
 yasmin_ros/yasmin_ros/service_state.py        | 14 +++++----
 3 files changed, 31 insertions(+), 13 deletions(-)

diff --git a/yasmin_ros/include/yasmin_ros/basic_outcomes.hpp b/yasmin_ros/include/yasmin_ros/basic_outcomes.hpp
index 3064cc6..048f4e6 100644
--- a/yasmin_ros/include/yasmin_ros/basic_outcomes.hpp
+++ b/yasmin_ros/include/yasmin_ros/basic_outcomes.hpp
@@ -24,6 +24,7 @@ namespace basic_outcomes {
 constexpr char SUCCEED[] = "succeeded";
 constexpr char ABORT[] = "aborted";
 constexpr char CANCEL[] = "canceled";
+constexpr char TIMEOUT[] = "timeout";
 
 } // namespace basic_outcomes
 } // namespace yasmin_ros
diff --git a/yasmin_ros/include/yasmin_ros/service_state.hpp b/yasmin_ros/include/yasmin_ros/service_state.hpp
index 7a7e727..1a92134 100644
--- a/yasmin_ros/include/yasmin_ros/service_state.hpp
+++ b/yasmin_ros/include/yasmin_ros/service_state.hpp
@@ -40,24 +40,29 @@ template <typename ServiceT> class ServiceState : public yasmin::State {
 public:
   ServiceState(std::string srv_name,
                CreateRequestHandler create_request_handler,
-               std::vector<std::string> outcomes)
-      : ServiceState(srv_name, create_request_handler, outcomes, nullptr) {}
+               std::vector<std::string> outcomes, int timeout = -1.0)
+      : ServiceState(srv_name, create_request_handler, outcomes, nullptr,
+                     timeout) {}
 
   ServiceState(std::string srv_name,
                CreateRequestHandler create_request_handler,
                std::vector<std::string> outcomes,
-               ResponseHandler response_handler)
+               ResponseHandler response_handler, int timeout = -1.0)
       : ServiceState(nullptr, srv_name, create_request_handler, outcomes,
-                     response_handler) {}
+                     response_handler, timeout) {}
 
   ServiceState(rclcpp::Node *node, std::string srv_name,
                CreateRequestHandler create_request_handler,
                std::vector<std::string> outcomes,
-               ResponseHandler response_handler)
-      : State({}) {
+               ResponseHandler response_handler, int timeout = -1.0)
+      : State({}), timeout(timeout) {
 
     this->outcomes = {basic_outcomes::SUCCEED, basic_outcomes::ABORT};
 
+    if (this->timeout >= 0) {
+      this->outcomes.push_back(basic_outcomes::TIMEOUT);
+    }
+
     if (outcomes.size() > 0) {
       for (std::string outcome : outcomes) {
         this->outcomes.push_back(outcome);
@@ -70,7 +75,8 @@ template <typename ServiceT> class ServiceState : public yasmin::State {
       this->node = node;
     }
 
-    this->service_client = this->node->template create_client<ServiceT>(srv_name);
+    this->service_client =
+        this->node->template create_client<ServiceT>(srv_name);
 
     this->create_request_handler = create_request_handler;
     this->response_handler = response_handler;
@@ -85,7 +91,13 @@ template <typename ServiceT> class ServiceState : public yasmin::State {
 
     Request request = this->create_request(blackboard);
 
-    this->service_client->wait_for_service();
+    bool serv_available = this->service_client->wait_for_service(
+        std::chrono::duration<int64_t, std::ratio<1>>(this->timeout));
+
+    if (!serv_available) {
+      return basic_outcomes::TIMEOUT;
+    }
+
     auto future = this->service_client->async_send_request(request);
 
     future.wait();
@@ -110,6 +122,7 @@ template <typename ServiceT> class ServiceState : public yasmin::State {
   std::shared_ptr<rclcpp::Client<ServiceT>> service_client;
   CreateRequestHandler create_request_handler;
   ResponseHandler response_handler;
+  int timeout;
 
   Request
   create_request(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) {
diff --git a/yasmin_ros/yasmin_ros/service_state.py b/yasmin_ros/yasmin_ros/service_state.py
index 5487ac7..a4ba23f 100644
--- a/yasmin_ros/yasmin_ros/service_state.py
+++ b/yasmin_ros/yasmin_ros/service_state.py
@@ -46,7 +46,11 @@ def __init__(
     ) -> None:
 
         self._srv_name = srv_name
-        _outcomes = [SUCCEED, ABORT, TIMEOUT]
+        _outcomes = [SUCCEED, ABORT]
+
+        self._timeout = timeout
+        if self._timeout:
+            _outcomes.append(TIMEOUT)
 
         if outcomes:
             _outcomes = _outcomes + outcomes
@@ -63,18 +67,18 @@ def __init__(
 
         if not self._create_request_handler:
             raise Exception("create_request_handler is needed")
-        
-        self._timeout = timeout
 
         super().__init__(_outcomes)
 
     def execute(self, blackboard: Blackboard) -> str:
 
         request = self._create_request_handler(blackboard)
-        serv_available = self._service_client.wait_for_service(timeout_sec = self._timeout)
+        serv_available = self._service_client.wait_for_service(
+            timeout_sec=self._timeout)
 
         if not serv_available:
-            self._node.get_logger().error("Specified timeout achieved. Service {} is not available and thus returning TIMEOUT outcome".format(self._srv_name))
+            self._node.get_logger().error(
+                "Specified timeout achieved. Service {} is not available and thus returning TIMEOUT outcome".format(self._srv_name))
             return TIMEOUT
 
         try: