Merge branch 'ros2' of github.com:RVMI/skiros2_std_lib into ros2

matthias-mayr · matthias-mayr · commit 7eb38d6753ea · 2024-02-29T14:05:24.000Z
diff --git a/skiros2_std_skills/skiros2_std_skills/action_client_primitive.py b/skiros2_std_skills/skiros2_std_skills/action_client_primitive.py
@@ -4,29 +4,13 @@
 import skiros2_common.tools.logger as log
 import rclpy
 from rclpy import action, task
+from rclpy.time import Duration
 from actionlib_msgs.msg import GoalStatus
 import queue
 from threading import Lock
 from typing import Optional
 from action_msgs.msg import GoalStatus
-
-class AtomicVar:
-    def __init__(self, initialValue: Optional[any]=None, sharedLock: Optional[Lock] = None) -> None:
-        self.lock = sharedLock
-        self.var = initialValue
-        if self.lock is None:
-            self.lock = Lock()
-    
-    def get_and_reset(self):
-        with self.lock:
-            var = self.var
-            if var is not None:
-                self.var = None
-            return var
-
-    def set(self, var):
-        with self.lock:
-            self.var = var
+from skiros2_std_skills.utils import AtomicVar
 
 class PrimitiveActionClient(PrimitiveBase):
     """
@@ -36,8 +20,17 @@ class PrimitiveActionClient(PrimitiveBase):
 
     build_client_onstart defines if the client will be built every time the skill it is called or not
     (save up to 0.3 seconds). Default is True
+
+    feedback_timeout_sec defines a timeout in seconds for not reciving action 
+    feedback in a timely manner. Could be used to detect if the action server 
+    has gone down or experiences issues. Default is no timeout.
     """
     build_client_onstart = True
+    feedback_timeout_sec = None # set to second if you want reliability
+
+    def __init__(self):
+        super().__init__()
+        self.client = None
 
     def onPreempt(self):
         """
@@ -60,17 +53,26 @@ def onStart(self):
         self._result_msg = AtomicVar()
         self._fail_status = AtomicVar()
 
-        if self.build_client_onstart:
+        if self.build_client_onstart or self.client is None:
             self.client = self.buildClient()
-            
-        if not self.client.wait_for_server(0.5):
-            return self.startError("Action server {} is not available.".format(self.client.action_client.ns), -101)
         
+        if self.client is None:
+            return self.startError("Action client returned by buildClient() is None.", -105)
+
+        if not self.client.wait_for_server(0.5):
+            return self.startError("Action server is not available: {}".format(self.client), -101)
+                
         self._goal_handle = None
-        self._goal_future = self.client.send_goal_async(self.buildGoal(), feedback_callback = self._feedback_callback)
+
+        goal = self.buildGoal()
+        if goal is None:
+            return self.startError("Action goal is None", -106)
+
+        self._goal_future = self.client.send_goal_async(goal, feedback_callback = self._feedback_callback)
         self._goal_future.add_done_callback(self._goal_request_callback)
         
         self._get_result_future = None
+        self._last_feedback_timestamp = self.node.get_clock().now()
 
         return True
 
@@ -82,12 +84,12 @@ def onStart(self):
     def execute(self):
         res = self._goal_msg.get_and_reset()
         if res is not None:
-            if not res:
-                # Rejected
+            if res == False:
                 return self.fail("Rejected.", -102)
 
         fb = self._feedback_msg.get_and_reset()
         if fb is not None:
+            self._last_feedback_timestamp = self.node.get_clock().now()
             return self.onFeedback(fb)
         
         status = self._fail_status.get_and_reset()
@@ -97,11 +99,16 @@ def execute(self):
         res = self._result_msg.get_and_reset()
         if res is not None:
             return self.onDone(res) # TODO: Remove None
+        
+        if self.feedback_timeout_sec is not None:
+            time_since_start_sec = (self.node.get_clock().now()-self._last_feedback_timestamp).nanoseconds/1.0e9 # type: Duration
+            log.info("Time since start %f, %d" % (time_since_start_sec, self.feedback_timeout_sec))
+            if time_since_start_sec > self.feedback_timeout_sec:
+                self.client.destroy()
+                self.client = None
+                return self.fail("Action feedback did not arrive within timeout of %d sec" % self.feedback_timeout_sec, -104)
 
         return State.Running
-
-    def get_result_msg(self):
-        return self.client.get_result()
     
     def _cancel_callback(self, future):
         cancel_response = future.result()
diff --git a/skiros2_std_skills/skiros2_std_skills/service_client_primitive.py b/skiros2_std_skills/skiros2_std_skills/service_client_primitive.py
@@ -0,0 +1,119 @@
+from skiros2_common.core.abstract_skill import State
+from skiros2_common.core.primitive import PrimitiveBase
+from skiros2_common.core.params import ParamTypes
+import skiros2_common.tools.logger as log
+import rclpy
+from rclpy import action, task, client, Future
+from rclpy.time import Duration
+from actionlib_msgs.msg import GoalStatus
+import queue
+from threading import Lock
+from typing import Optional
+from action_msgs.msg import GoalStatus
+from skiros2_std_skills.utils import AtomicVar
+
+class PrimitiveServiceClient(PrimitiveBase):
+    """
+    @brief Base class for skills based on a service client.
+
+    See test_service_skill for a practical example
+    """
+
+    build_client_onstart = False
+    call_timeout_sec = None
+
+    def onPreempt(self):
+        """
+        @brief Cancel service call
+        """
+        log.debug("Cancel requested")
+        if self._future is not None:
+            self._future.cancel()
+            self._future = None
+            self.client.destroy()
+            self.client = None
+            log.debug("Service call canceled")
+            return self.success("Service call canceled.")
+        else:
+            return self.fail("Goal has not been accepted or rejected, cannot cancel.")
+
+    def onStart(self):
+        self._response = AtomicVar()
+
+        # 1. Build client
+        if self.build_client_onstart or self.client is None:
+            self.client = self.buildClient()
+
+        if self.client is None:
+            return self.startError("Action client returned by buildClient() is None.", -101)
+
+        # 2. Wait for server
+        if not self.client.wait_for_service(0.5):
+            return self.startError("Service server is not available: {}".format(self.client), -101)
+
+        # 3. Build message and call service
+        service_request = self.buildRequest()
+        if service_request is None:
+            return self.startError("Service request is None", -106)
+
+        self._future = self.client.call_async(service_request)
+        self._future.add_done_callback(self._response_callback)
+        self._last_call_timestamp = self.node.get_clock().now()
+
+        return True
+    
+    def _response_callback(self, future: Future):
+        self._response.set(future.result())
+
+    def execute(self):
+        res = self._response.get_and_reset()
+        if res is not None:
+            return self.onDone(res)
+        
+        time_since_start_sec = (self.node.get_clock().now()-self._last_call_timestamp).nanoseconds/1.0e9
+        if self.call_timeout_sec is not None:
+            if time_since_start_sec > self.call_timeout_sec:
+                self.client.destroy()
+                self.client = None
+                return self.fail("Service call did not finish within timeout of %d sec" % self.call_timeout_sec, -104)
+
+        return self.step("Service called, been waiting for %.1f sec" % time_since_start_sec)
+
+    def onInit(self):
+        """
+        @brief Optional to override. Called once when loading the primitive. If return False, the primitive is not loaded
+        @return True if loaded correctly. False on error
+        """
+        self.client = None
+        if not self.build_client_onstart:
+            self.client = self.buildClient()
+
+        return True
+
+    def onEnd(self):
+        """
+        @brief Optional to override. Routine called just after skill execution end
+        """
+        return True
+
+    def buildClient(self)->client.Client:
+        """
+        @brief To override. Called when starting the skill
+        @return an service client created by self.node.create_client(...)
+        """
+        pass
+
+    def buildRequest(self):
+        """
+        @brief To override. Called when starting the skill
+        @return an action msg initialized
+        """
+        pass
+
+    def onDone(self, response):
+        """
+        @brief To override. Called when response has arrived.
+        @return self.success or self.fail
+        """
+        #Do something with result msg
+        return self.success("Finished. Result: {}".format(response))
diff --git a/skiros2_std_skills/skiros2_std_skills/utils.py b/skiros2_std_skills/skiros2_std_skills/utils.py
@@ -0,0 +1,25 @@
+import queue
+from threading import Lock
+from typing import Optional
+
+class AtomicVar:
+    def __init__(self, initialValue: Optional[any]=None, sharedLock: Optional[Lock] = None) -> None:
+        self.lock = sharedLock
+        self.var = initialValue
+        if self.lock is None:
+            self.lock = Lock()
+    
+    def get_and_reset(self):
+        with self.lock:
+            var = self.var
+            if var is not None:
+                self.var = None
+            return var
+
+    def get(self):
+        with self.lock:
+            return self.var
+
+    def set(self, var):
+        with self.lock:
+            self.var = var
