Refactor cancellable task into future results

Author: gonzalocasas

CHANGELOG.rst

@@ -16,13 +16,16 @@ Unreleased
 * Changed example for loading PosConCM (includes parity argument, differs from PosCon3D)
 * Changed format ``compas_fab.sensors.baumer.PosConCM.set_flex_mount()``
 * Changed tasks.py to run ``invoke test``
+* Renamed ``compas_fab.backends.CancellableTask`` to ``compas_fab.backends.CancellableFutureResult``
 * ROS client: changed joint trajectory follower (``follow_joint_trajectory``) to support generic ``JointTrajectory`` arguments.
+* ROS client: changed return type of trajectory execution methods to ``CancellableFutureResult``
 
 **Added**
 
 * Added ``compas_fab.sensors.baumer.PosCon3D.reset()``
 * Added ``compas_fab.sensors.baumer.PosConCM.reset()``
 * ROS client: added support for MoveIt! execution action via `client.execute_joint_trajectory`.
+* Added ``compas_fab.backends.FutureResult`` class to deal with long-running async tasks
 
 **Removed**
 

src/compas_fab/backends/__init__.py

@@ -26,14 +26,15 @@
     RosClient
     RosFileServerLoader
 
-Tasks
------
+Long-running tasks
+------------------
 
 .. autosummary::
     :toctree: generated/
     :nosignatures:
 
-    CancellableTask
+    FutureResult
+    CancellableFutureResult
 
 Exceptions
 ----------

src/compas_fab/backends/ros/client.py

@@ -10,15 +10,17 @@
 from compas_fab.backends.ros.messages import ExecuteTrajectoryFeedback
 from compas_fab.backends.ros.messages import ExecuteTrajectoryGoal
 from compas_fab.backends.ros.messages import ExecuteTrajectoryResult
+from compas_fab.backends.ros.messages import FollowJointTrajectoryFeedback
 from compas_fab.backends.ros.messages import FollowJointTrajectoryGoal
 from compas_fab.backends.ros.messages import FollowJointTrajectoryResult
 from compas_fab.backends.ros.messages import Header
 from compas_fab.backends.ros.messages import JointTrajectory as RosMsgJointTrajectory
 from compas_fab.backends.ros.messages import JointTrajectoryPoint as RosMsgJointTrajectoryPoint
+from compas_fab.backends.ros.messages import MoveItErrorCodes
 from compas_fab.backends.ros.messages import RobotTrajectory
 from compas_fab.backends.ros.messages import Time
 from compas_fab.backends.ros.planner_backend_moveit import MoveItPlanner
-from compas_fab.backends.tasks import CancellableTask
+from compas_fab.backends.tasks import CancellableFutureResult
 
 __all__ = [
     'RosClient',
@@ -29,19 +31,29 @@
 }
 
 
-class CancellableRosAction(CancellableTask):
+class CancellableRosActionResult(CancellableFutureResult):
     def __init__(self, goal):
+        super(CancellableRosActionResult, self).__init__()
         self.goal = goal
 
     def cancel(self):
-        """Attempt to cancel the task.
+        """Attempt to cancel the action.
 
         If the task is currently being executed and cannot be cancelled
         then the method will return ``False``, otherwise the call will
         be cancelled and the method will return ``True``.
         """
+        if self.done:
+            raise Exception('Already completed action cannot be cancelled')
+
+        if self.goal.is_finished:
+            return False
+
         self.goal.cancel()
 
+        # NOTE: Check if we can output more meaning results than just "we tried to cancel"
+        return True
+
 
 class RosClient(Ros):
     """Interface to use ROS as backend via the **rosbridge**.
@@ -302,32 +314,43 @@ def follow_joint_trajectory(self, joint_trajectory, action_name='/joint_trajecto
         joint_trajectory = self._convert_to_ros_trajectory(joint_trajectory)
         trajectory_goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory)
 
-        def handle_result(msg, client):
+        action_client = ActionClient(self, action_name, 'control_msgs/FollowJointTrajectoryAction')
+        goal = Goal(action_client, Message(trajectory_goal.msg))
+        action_result = CancellableRosActionResult(goal)
+
+        def handle_result(msg):
             result = FollowJointTrajectoryResult.from_msg(msg)
-            callback(result)
+            if result.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
+                ros_error = RosError(
+                    'Follow trajectory failed={}'.format(result.error_string),
+                    int(result.error_code))
+                action_result._set_error_result(ros_error)
+                if errback:
+                    errback(ros_error)
+            else:
+                action_result._set_result(result)
+                if callback:
+                    callback(result)
+
+        def handle_feedback(msg):
+            feedback = FollowJointTrajectoryFeedback.from_msg(msg)
+            feedback_callback(feedback)
 
         def handle_failure(error):
             errback(error)
 
-        action_client = ActionClient(self, action_name,
-                                     'control_msgs/FollowJointTrajectoryAction')
-
-        goal = Goal(action_client, Message(trajectory_goal.msg))
-
-        if callback:
-            goal.on('result', lambda result: handle_result(
-                result, action_client))
+        goal.on('result', handle_result)
 
         if feedback_callback:
-            goal.on('feedback', feedback_callback)
+            goal.on('feedback', handle_feedback)
 
         if errback:
             goal.on('timeout', lambda: handle_failure(
-                RosError("Action Goal timeout", -1)))
+                RosError('Action Goal timeout', -1)))
 
         goal.send(timeout=timeout)
 
-        return CancellableRosAction(goal)
+        return action_result
 
     def execute_joint_trajectory(self, joint_trajectory, action_name='/execute_trajectory', callback=None, errback=None, feedback_callback=None, timeout=60000):
         """Execute a joint trajectory via the MoveIt infrastructure.
@@ -355,17 +378,29 @@ def execute_joint_trajectory(self, joint_trajectory, action_name='/execute_traje
 
         Returns
         -------
-        :class:`CancellableTask`
-            An instance of a cancellable tasks.
+        :class:`CancellableFutureResult`
+            An instance of a cancellable future result.
         """
 
         joint_trajectory = self._convert_to_ros_trajectory(joint_trajectory)
         trajectory = RobotTrajectory(joint_trajectory=joint_trajectory)
         trajectory_goal = ExecuteTrajectoryGoal(trajectory=trajectory)
 
+        action_client = ActionClient(self, action_name, 'moveit_msgs/ExecuteTrajectoryAction')
+        goal = Goal(action_client, Message(trajectory_goal.msg))
+        action_result = CancellableRosActionResult(goal)
+
         def handle_result(msg):
             result = ExecuteTrajectoryResult.from_msg(msg)
-            callback(result)
+            if result.error_code != MoveItErrorCodes.SUCCESS:
+                ros_error = RosError('Execute trajectory failed. Message={}'.format(result.error_code.human_readable), int(result.error_code))
+                action_result._set_error_result(ros_error)
+                if errback:
+                    errback(ros_error)
+            else:
+                action_result._set_result(result)
+                if callback:
+                    callback(result)
 
         def handle_feedback(msg):
             feedback = ExecuteTrajectoryFeedback.from_msg(msg)
@@ -374,18 +409,14 @@ def handle_feedback(msg):
         def handle_failure(error):
             errback(error)
 
-        action_client = ActionClient(self, action_name, 'moveit_msgs/ExecuteTrajectoryAction')
-        goal = Goal(action_client, Message(trajectory_goal.msg))
-
-        if callback:
-            goal.on('result', handle_result)
+        goal.on('result', handle_result)
 
         if feedback_callback:
             goal.on('feedback', handle_feedback)
 
         if errback:
-            goal.on('timeout', lambda: handle_failure(RosError("Action Goal timeout", -1)))
+            goal.on('timeout', lambda: handle_failure(RosError('Action Goal timeout', -1)))
 
         goal.send(timeout=timeout)
 
-        return CancellableRosAction(goal)
+        return action_result

src/compas_fab/backends/tasks.py

@@ -2,19 +2,74 @@
 from __future__ import division
 from __future__ import print_function
 
+import threading
+
 __all__ = [
-    'CancellableTask',
+    'FutureResult',
+    'CancellableFutureResult'
 ]
 
 
-class CancellableTask(object):
-    """Preemptable task represents a long-running operation that can be cancelled."""
+class FutureResult(object):
+    """Represents a future result value.
+
+    Futures are the result of asynchronous operations
+    but allow to explicitely control when to block and wait
+    for its completion."""
+
+    def __init__(self):
+        self.done = False
+        self.value = None
+        self.error = None
+        self.event = threading.Event()
+
+    def result(self, timeout=None):
+        """Return the feedback value returned by the instruction.
+
+        If the instruction has not yet returned feedback, it will wait
+        up to ``timeout`` seconds. If the ``timeout`` expires, the method
+        will raise an exception.
+        """
+        if not self.done:
+            if not self.event.wait(timeout):
+                raise Exception('Timeout: future result not available')
+
+        if self.error:
+            raise self.error
+
+        return self.value
+
+    def _set_result(self, message):
+        self.done = True
+        self.value = message
+        self.error = None
+        self.event.set()
+
+    def _set_error_result(self, error):
+        """Mark the future as failed.
+
+        Parameters
+        ----------
+        error : Exception
+            An exception instance.
+        """
+        self.done = True
+        self.value = None
+        self.error = error
+        self.event.set()
+
+
+class CancellableFutureResult(FutureResult):
+    """Represents a future result of a long-running asynchronous operation that can be cancelled."""
+
+    def __init__(self):
+        super(CancellableFutureResult, self).__init__()
 
     def cancel(self):
-        """Attempt to cancel the task.
+        """Attempt to cancel the operation.
 
-        If the task is currently being executed and cannot be cancelled
+        If the operation is currently being executed and cannot be cancelled
         then the method will return ``False``, otherwise the call will
         be cancelled and the method will return ``True``.
         """
-        raise NotImplementedError('Concrete tasks need to provide an implementation')
+        raise NotImplementedError('Needs concrete implementation')