diff --git a/yasmin_ros/yasmin_ros/basic_outcomes.py b/yasmin_ros/yasmin_ros/basic_outcomes.py index a9410a1..f67c0f1 100644 --- a/yasmin_ros/yasmin_ros/basic_outcomes.py +++ b/yasmin_ros/yasmin_ros/basic_outcomes.py @@ -17,3 +17,4 @@ SUCCEED = "succeeded" ABORT = "aborted" CANCEL = "canceled" +TIMEOUT = "timeout" diff --git a/yasmin_ros/yasmin_ros/service_state.py b/yasmin_ros/yasmin_ros/service_state.py index f329a85..5487ac7 100644 --- a/yasmin_ros/yasmin_ros/service_state.py +++ b/yasmin_ros/yasmin_ros/service_state.py @@ -22,7 +22,7 @@ from yasmin import State from yasmin import Blackboard from yasmin_ros.yasmin_node import YasminNode -from yasmin_ros.basic_outcomes import SUCCEED, ABORT +from yasmin_ros.basic_outcomes import SUCCEED, ABORT, TIMEOUT class ServiceState(State): @@ -31,6 +31,8 @@ class ServiceState(State): _service_client: Client _create_request_handler: Callable _response_handler: Callable + _timeout: float + _srv_name: str def __init__( self, @@ -39,10 +41,12 @@ def __init__( create_request_handler: Callable, outcomes: List[str] = None, response_handler: Callable = None, - node: Node = None + node: Node = None, + timeout: float = None ) -> None: - _outcomes = [SUCCEED, ABORT] + self._srv_name = srv_name + _outcomes = [SUCCEED, ABORT, TIMEOUT] if outcomes: _outcomes = _outcomes + outcomes @@ -59,13 +63,19 @@ 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) - self._service_client.wait_for_service() + 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)) + return TIMEOUT try: response = self._service_client.call(request)