158: WIP general service client

src/lhw_intelligence/lhw_intelligence/new_behaviours/service_client.py

+
+
+
+
+
+##############################################################################
+# Imports
+##############################################################################
+
+from os import access
+import typing
+import py_trees
+import lhw_interfaces.msg as lhw_msgs
+import lhw_interfaces.srv as lhw_srvs
+from py_trees_ros import exceptions
+import rclpy
+
+##############################################################################
+# Behaviours
+##############################################################################
+
+class ServiceFromBlackboard(py_trees.behaviour.Behaviour):
+    """
+    """
+    def __init__(
+        self,
+        name: str,
+        key: str,
+        service_name: str,
+        service_type: typing.Any,
+        generate_feedback_message: typing.Callable[[typing.Any], str]=None,
+        wait_for_service_timeout_sec: float=-3.0 
+    ):
+        super(ServiceFromBlackboard, self).__init__(name=name)
+        self.service_name = service_name
+        self.service_type = service_type
+        self.wait_for_service_timeout_sec = wait_for_service_timeout_sec
+        self.generate_feedback_message = generate_feedback_message
+
+        self.blackboard = self.attach_blackboard_client(name=self.name)
+        self.blackboard.register_key(
+            key="request",
+            access=py_trees.common.Access.READ,
+            remap_to=py_trees.blackboard.Blackboard.absolute_name("/%s_request", key)
+        )
+        self.blackboard.register_key(
+            key="result",
+            access=py_trees.common.Access.WRITE,
+            remap_to=py_trees.blackboard.Blackboard.absolute_name("/%s_result", key)
+        )
+
+
+    def setup(self, **kwargs):
+        """
+        Setup the service client.
+        Args:
+            **kwargs (:obj:`dict`): distribute arguments to this
+                behaviour and in turn, all of it's children
+        Raises:
+            :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+            :class:`~py_trees_ros.exceptions.TimedOutError`: if the action server could not be found
+        """
+        self.logger.debug("{}.setup()".format(self.qualified_name))
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability
+
+        self.service_client = self.node.create_client(self.service_type, self.service_name)
+        result = None
+        if self.wait_for_service_timeout_sec > 0.0:
+            result = self.service_client.wait_for_server(timeout_sec=self.wait_for_service_timeout_sec)
+        else:
+            iterations = 0
+            period_sec = -1.0*self.wait_for_server_timeout_sec
+            while not result:
+                iterations += 1
+                result = self.service_client.wait_for_server(timeout_sec=period_sec)
+                if not result:
+                    self.node.get_logger().warning(
+                        "waiting for action server ... [{}s][{}][{}]".format(
+                            iterations * period_sec,
+                            self.action_name,
+                            self.qualified_name
+                        )
+                    )
+        if not result:
+            self.feedback_message = "timed out waiting for the server [{}]".format(self.service_name)
+            self.node.get_logger().error("{}[{}]".format(self.feedback_message, self.qualified_name))
+            raise exceptions.TimedOutError(self.feedback_message)
+        else:
+            self.feedback_message = "... connected to action server [{}]".format(self.service_name)
+            self.node.get_logger().info("{}[{}]".format(self.feedback_message, self.qualified_name))        
+
+    def initialise(self):
+        """
+        Reset the internal variables and kick off a new request.
+        """
+        self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+        # initialise some temporary variables
+        self.request_handle = None
+        self.get_result_future = None
+
+        self.result_message = None
+
+        self.blackboard.feedback = None
+        self.blackboard.result = None
+
+        try:
+            self._send_request(self.blackboard.request)
+            self.feedback_message = "sent request"
+        except KeyError:
+            pass # self.send_request_future will be None, check on that
+
+    def update(self):
+        """
+        Check only to see whether the underlying service server has
+        succeeded, is running, or has cancelled/aborted for som reason and
+        map these to the usual behaviour return states.
+        Returns:
+            :class:`py_trees.common.Status`
+        """
+        self.logger.debug("{}.update()".format(self.qualified_name))
+
+        if self.get_result_future is None:
+            self.feedback_message = "request failed :[ [{}]\n{!r}".format(
+                self.qualified_name, 
+                self.get_result_future.exception()
+            )
+            self.node.get_logger().debug('... {}'.format(self.feedback_message))
+            return py_trees.common.Status.FAILURE
+        self.request_handle = self.get_result_future.result()
+        if not self.request_handle.accepted:
+            self.feedback_message = "request rejected :( [{}]".format(self.qualified_name)
+            self.node.get_logger().debug('... {}'.format(self.feedback_message))
+            return py_trees.common.Status.FAILURE
+        # CONTINUE HERE!
+
+        
+
+
+    def _send_request(self, request: typing.Any):
+        """
+        Send the request, get a future back and start lining up the
+        chain of callbacks that will lead to a result.
+        """
+        self.feedback_message = "sending goal ..."
+        self.node.get_logger().debug("{} [{}]".format(
+            self.feedback_message,
+            self.qualified_name
+        ))
+        self.get_result_future = self.service_client.call_async(request)
+        self.get_result_future.add_done_callback(self._get_result_callback)
+
+    def _get_result_callback(self, future: rclpy.task.Future):
+        """
+        Immediate callback for the result, saves data into local variables so
+        the update method can react accordingly.
+        Args:
+            future: incoming request result delivered from the service server
+        """
+        self.blackboard.result = future.result()
+        self.result_message = future.result()
\ No newline at end of file