[behaviours] wait_for_server_timeout in action clients (#152)

CHANGELOG.rst

@@ -4,6 +4,7 @@ Changelog
 
 Forthcoming
 -----------
+* [behaviours] wait_for_server_timeout_sec in action clients, `#152 <https://github.com/splintered-reality/py_trees_ros/pull/152>`_
 * [trees] avoid irregular snapshot streams by avoiding timing jitter, `#151 <https://github.com/splintered-reality/py_trees_ros/pull/151>`_
 
 2.0.7 (2020-02-10)

py_trees_ros/action_clients.py

@@ -7,6 +7,7 @@
 ##############################################################################
 # Documentation
 ##############################################################################
+from asyncio.tasks import wait_for
 
 """
 Behaviours for ROS actions.
@@ -83,17 +84,20 @@ class FromBlackboard(py_trees.behaviour.Behaviour):
         name: name of the behaviour (default: lowercase class name)
         generate_feedback_message: formatter for feedback messages, takes action_type.Feedback
             messages and returns strings (default: None)
+        wait_for_server_timeout_sec: how long to wait in setup for a server connection
     """
     def __init__(self,
                  action_type: typing.Any,
                  action_name: str,
                  key: str,
                  name: str=py_trees.common.Name.AUTO_GENERATED,
                  generate_feedback_message: typing.Callable[[typing.Any], str]=None,
+                 wait_for_server_timeout_sec: float=2.0
                  ):
         super().__init__(name)
         self.action_type = action_type
         self.action_name = action_name
+        self.wait_for_server_timeout_sec = wait_for_server_timeout_sec
         self.blackboard = self.attach_blackboard_client(name=self.name)
         self.blackboard.register_key(
             key="goal",
@@ -145,7 +149,7 @@ def setup(self, **kwargs):
                 self.action_name, self.qualified_name
             )
         )
-        result = self.action_client.wait_for_server(timeout_sec=2.0)
+        result = self.action_client.wait_for_server(timeout_sec=self.wait_for_server_timeout_sec)
         if not result:
             self.feedback_message = "timed out waiting for the server [{}]".format(self.action_name)
             self.node.get_logger().error("{}[{}]".format(self.feedback_message, self.qualified_name))
@@ -355,13 +359,16 @@ class FromConstant(FromBlackboard):
         name: name of the behaviour (default: lowercase class name)
         generate_feedback_message: formatter for feedback messages, takes action_type.Feedback
             messages and returns strings (default: None)
+        wait_for_server_timeout_sec: how long to wait in setup for a server connection
+                 wait_for_server_timeout_sec: float=2.0
     """
     def __init__(self,
                  action_type: typing.Any,
                  action_name: str,
                  action_goal: typing.Any,
                  name: str=py_trees.common.Name.AUTO_GENERATED,
                  generate_feedback_message: typing.Callable[[typing.Any], str]=None,
+                 wait_for_server_timeout_sec: float=2.0
                  ):
         unique_id = uuid.uuid4()
         key = "/goal_" + str(unique_id)
@@ -370,7 +377,8 @@ def __init__(self,
             action_name=action_name,
             key=key,
             name=name,
-            generate_feedback_message=generate_feedback_message
+            generate_feedback_message=generate_feedback_message,
+            wait_for_server_timeout_sec=wait_for_server_timeout_sec
         )
         # parent already instantiated a blackboard client
         self.blackboard.register_key(

tests/test_action_client.py

@@ -16,6 +16,7 @@
 import rclpy
 import rclpy.action
 import rclpy.executors
+import time
 
 ##############################################################################
 # Helpers
@@ -34,22 +35,24 @@ def assert_details(text, expected, result):
           console.reset)
 
 
-def create_action_client(from_blackboard=False):
+def create_action_client(from_blackboard=False, wait_for_server_timeout_sec=2.0):
     if from_blackboard:
         behaviour = py_trees_ros.action_clients.FromBlackboard(
             name="dock",
             action_type=py_trees_actions.Dock,
             action_name="dock",
             key="goal",
-            generate_feedback_message=lambda msg: "{:.2f}%%".format(msg.feedback.percentage_completed)
+            generate_feedback_message=lambda msg: "{:.2f}%%".format(msg.feedback.percentage_completed),
+            wait_for_server_timeout_sec=wait_for_server_timeout_sec
         )
     else:
         behaviour = py_trees_ros.action_clients.FromConstant(
             name="dock",
             action_type=py_trees_actions.Dock,
             action_name="dock",
             action_goal=py_trees_actions.Dock.Goal(dock=True),  # noqa
-            generate_feedback_message=lambda msg: "{:.2f}%%".format(msg.feedback.percentage_completed)
+            generate_feedback_message=lambda msg: "{:.2f}%%".format(msg.feedback.percentage_completed),
+            wait_for_server_timeout_sec=wait_for_server_timeout_sec
         )
     return behaviour
 
@@ -290,7 +293,7 @@ def test_aborted():
     executor.shutdown()
 
 ########################################
-# Aborted
+# From Blackboard
 ########################################
 
 
@@ -332,3 +335,24 @@ def test_from_blackboard():
     server.shutdown()
     executor.shutdown()
 
+########################################
+# Timeouts
+########################################
+
+
+def test_timeouts():
+    console.banner("Timeouts")
+
+    timeout = 0.1
+    root = create_action_client(from_blackboard=True, wait_for_server_timeout_sec=timeout)
+    tree = py_trees_ros.trees.BehaviourTree(root=root)
+
+    start_time = time.monotonic()
+    try:
+        tree.setup()
+    except py_trees_ros.exceptions.TimedOutError:
+        duration = time.monotonic() - start_time
+    assert_details("Tree Setup Timeout", "delta+{}".format(timeout), duration)
+    assert(duration < 10*timeout)
+
+    tree.shutdown()