[action_client] failure modes

py_trees_ros/actions.py

@@ -58,13 +58,6 @@ def __init__(self,
 
         self.node = None
         self.action_client = None
-        self.goal_handle = None
-        self.send_goal_future = None
-        self.get_result_future = None
-
-        self.result_message = None
-        self.result_status = None
-        self.result_status_string = None
 
         self.status_strings = {
                 action_msgs.GoalStatus.STATUS_UNKNOWN : "STATUS_UNKNOWN",  # noqa
@@ -119,6 +112,16 @@ def initialise(self):
         Reset the internal variables.
         """
         self.logger.debug("{}.initialise()".format(self.qualified_name))
+
+        # initialise some temporary variables
+        self.goal_handle = None
+        self.send_goal_future = None
+        self.get_result_future = None
+
+        self.result_message = None
+        self.result_status = None
+        self.result_status_string = None
+
         self.feedback_message = "sent goal request"
         self.send_goal_request()
 
@@ -130,6 +133,10 @@ def update(self):
         """
         self.logger.debug("{}.update()".format(self.qualified_name))
 
+        if self.goal_handle is not None and not self.goal_handle.accepted:
+            # goal was rejected
+            self.feedback_message = "goal rejected"
+            return py_trees.common.Status.FAILURE
         if self.result_status is None:
             return py_trees.common.Status.RUNNING
         elif not self.get_result_future.done():
@@ -208,9 +215,13 @@ def goal_response_callback(self, future):
         """
         Handle goal response, proceed to listen for the result if accepted.
         """
+        if future.result() is None:
+            self.feedback_message = "goal request failed :[ [{}]\n{!r}".format(self.qualified_name, future.exception())
+            self.node.get_logger().info('... {}'.format(self.feedback_message))
+            return
         self.goal_handle = future.result()
         if not self.goal_handle.accepted:
-            self.feedback_message = "goal rejected :( [{}]\n{!r}".format(self.qualified_name, future.exception())
+            self.feedback_message = "goal rejected :( [{}]".format(self.qualified_name)
             self.node.get_logger().info('... {}'.format(self.feedback_message))
             return
         else:

tests/test_action_client.py

@@ -9,14 +9,13 @@
 # Imports
 ##############################################################################
 
-import action_msgs.msg as action_msgs  # GoalStatus
 import py_trees
 import py_trees.console as console
 import py_trees_ros
 import py_trees_ros_interfaces.action as py_trees_actions
 import rclpy
+import rclpy.action
 import rclpy.executors
-import time
 import unittest
 
 ##############################################################################
@@ -57,6 +56,37 @@ def print_unicode_tree(tree):
         )
     )
 
+
+class RejectGoalServer(object):
+    def __init__(self, node_name, action_name, action_type):
+        self.node = rclpy.create_node(node_name)
+
+        self.action_server = rclpy.action.ActionServer(
+            node=self.node,
+            action_type=action_type,
+            action_name=action_name,
+            callback_group=rclpy.callback_groups.ReentrantCallbackGroup(),  # needed?
+            execute_callback=lambda goal_handle: None,
+            goal_callback=self.goal_callback
+        )
+
+    def goal_callback(self, unused_goal_request):
+        return rclpy.action.server.GoalResponse.REJECT
+
+    def shutdown(self):
+        self.action_server.destroy()
+        self.node.destroy_node()
+
+
+class DockFailedServer(py_trees_ros.mock.dock.Dock):
+    def __init__(self):
+        super().__init__()
+
+    def execute_goal_callback(self, goal_handle):
+        result = self.action_type.Result()
+        goal_handle.set_aborted()
+        return result
+
 ##############################################################################
 # Tests
 ##############################################################################
@@ -71,6 +101,7 @@ def setUpClass(cls):
 
         cls.timeout = 3.0
         cls.blackboard = py_trees.blackboard.Blackboard()
+        cls.number_of_iterations = 100
 
     @classmethod
     def tearDownClass(cls):
@@ -80,9 +111,9 @@ def tearDownClass(cls):
     def setUp(self):
         pass
 
-##############################################################################
-# Success
-##############################################################################
+    ########################################
+    # Success
+    ########################################
 
     def test_success(self):
         console.banner("Client Success")
@@ -110,13 +141,12 @@ def test_success(self):
         assert_details("root.status", "RUNNING", root.status)
         self.assertEqual(root.status, py_trees.common.Status.RUNNING)
 
-        number_of_iterations = 100
         tree.tick_tock(
             period_ms=100,
-            number_of_iterations=number_of_iterations
+            number_of_iterations=self.number_of_iterations
         )
 
-        while tree.count < number_of_iterations and root.status == py_trees.common.Status.RUNNING:
+        while tree.count < self.number_of_iterations and root.status == py_trees.common.Status.RUNNING:
             executor.spin_once(timeout_sec=0.05)
 
         assert_details("root.status", "SUCCESS", root.status)
@@ -126,11 +156,13 @@ def test_success(self):
         server.shutdown()
         executor.shutdown()
 
+    ########################################
+    # Priority Interrupt
+    ########################################
+
     def test_priority_interrupt(self):
         console.banner("Priority Interrupt")
 
-        number_of_iterations = 50
-
         server = py_trees_ros.mock.dock.Dock(duration=1.5)
 
         action_client = create_action_client()
@@ -158,10 +190,10 @@ def test_priority_interrupt(self):
 
         tree.tick_tock(
             period_ms=100,
-            number_of_iterations=number_of_iterations
+            number_of_iterations=self.number_of_iterations
         )
 
-        while tree.count < number_of_iterations and "cancelled" not in action_client.feedback_message:
+        while tree.count < self.number_of_iterations and "cancelled" not in action_client.feedback_message:
             executor.spin_once(timeout_sec=0.1)
         print_unicode_tree(tree)
         assert_details("action_client.status", "INVALID", action_client.status)
@@ -177,12 +209,78 @@ def test_priority_interrupt(self):
         server.shutdown()
         executor.shutdown()
 
-##############################################################################
-# Preemption
-##############################################################################
+    ########################################
+    # Rejection
+    ########################################
+
+    def test_rejection(self):
+        console.banner("Client Rejection")
+
+        server = RejectGoalServer(
+            node_name="reject",
+            action_name="dock",
+            action_type=py_trees_actions.Dock,
+        )
+
+        root = create_action_client()
+        tree = py_trees_ros.trees.BehaviourTree(root=root)
+
+        # ROS Setup
+        tree.setup()
+        executor = rclpy.executors.MultiThreadedExecutor(num_threads=4)
+        executor.add_node(server.node)
+        executor.add_node(tree.node)
+        tree.tick()
+        tree.tick_tock(period_ms=100, number_of_iterations=self.number_of_iterations)
+
+        # ROS Spin
+        while tree.count < self.number_of_iterations and root.status == py_trees.common.Status.RUNNING:
+            executor.spin_once(timeout_sec=0.05)
+
+        print("")
+        assert_banner()
+        assert_details("root.status", "FAILURE", root.status)
+        self.assertEqual(root.status, py_trees.common.Status.FAILURE)
+
+        tree.shutdown()
+        server.shutdown()
+        executor.shutdown()
+
+    ########################################
+    # Aborted
+    ########################################
+
+    def test_aborted(self):
+        console.banner("Server Aborted")
+
+        server = DockFailedServer()
+
+        root = create_action_client()
+        tree = py_trees_ros.trees.BehaviourTree(root=root)
+
+        # ROS Setup
+        tree.setup()
+        executor = rclpy.executors.MultiThreadedExecutor(num_threads=4)
+        executor.add_node(server.node)
+        executor.add_node(tree.node)
+        tree.tick()
+        tree.tick_tock(period_ms=100, number_of_iterations=self.number_of_iterations)
+
+        # ROS Spin
+        while tree.count < self.number_of_iterations and root.status == py_trees.common.Status.RUNNING:
+            executor.spin_once(timeout_sec=0.05)
+
+        print("")
+        assert_banner()
+        assert_details("root.status", "FAILURE", root.status)
+        self.assertEqual(root.status, py_trees.common.Status.FAILURE)
+
+        tree.shutdown()
+        server.shutdown()
+        executor.shutdown()
 
 ##############################################################################
-# Cancel
+# Main
 ##############################################################################