Add support for execute joint trajectory

gonzalocasas · gonzalocasas · commit 1da319ba7b2e · 2019-10-17T02:57:38.000+02:00
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -22,6 +22,7 @@ Unreleased
 
 * 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`.
 
 **Removed**
 
diff --git a/src/compas_fab/backends/ros/client.py b/src/compas_fab/backends/ros/client.py
@@ -7,11 +7,15 @@
 from roslibpy.actionlib import Goal
 
 from compas_fab.backends.ros.exceptions import RosError
+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 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 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
@@ -324,3 +328,64 @@ def handle_failure(error):
         goal.send(timeout=timeout)
 
         return CancellableRosAction(goal)
+
+    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.
+
+        Note
+        ----
+        This method does not support Multi-DOF Joint Trajectories.
+
+        Parameters
+        ----------
+        joint_trajectory : :class:`compas_fab.robots.JointTrajectory`
+            Instance of joint trajectory.
+        callback : callable
+            Function to be invoked when the goal is completed, requires
+            one positional parameter ``result``.
+        action_name : string
+            ROS action name, defaults to ``/execute_trajectory``.
+        errback : callable
+            Function to be invoked in case of error or timeout, requires
+            one position parameter ``exception``.
+        feedback_callback : callable
+            Function to be invoked during execution to provide feedback.
+        timeout : int
+            Timeout for goal completion in milliseconds.
+
+        Returns
+        -------
+        :class:`CancellableTask`
+            An instance of a cancellable tasks.
+        """
+
+        joint_trajectory = self._convert_to_ros_trajectory(joint_trajectory)
+        trajectory = RobotTrajectory(joint_trajectory=joint_trajectory)
+        trajectory_goal = ExecuteTrajectoryGoal(trajectory=trajectory)
+
+        def handle_result(msg):
+            result = ExecuteTrajectoryResult.from_msg(msg)
+            callback(result)
+
+        def handle_feedback(msg):
+            feedback = ExecuteTrajectoryFeedback.from_msg(msg)
+            feedback_callback(feedback)
+
+        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)
+
+        if feedback_callback:
+            goal.on('feedback', handle_feedback)
+
+        if errback:
+            goal.on('timeout', lambda: handle_failure(RosError("Action Goal timeout", -1)))
+
+        goal.send(timeout=timeout)
+
+        return CancellableRosAction(goal)
diff --git a/src/compas_fab/backends/ros/messages/moveit_msgs.py b/src/compas_fab/backends/ros/messages/moveit_msgs.py
@@ -1,21 +1,21 @@
 from __future__ import absolute_import
 
-from compas_fab.backends.ros.messages.std_msgs import ROSmsg
-from compas_fab.backends.ros.messages.std_msgs import Header
 from compas_fab.backends.ros.messages.geometry_msgs import Point
 from compas_fab.backends.ros.messages.geometry_msgs import Pose
 from compas_fab.backends.ros.messages.geometry_msgs import PoseStamped
-from compas_fab.backends.ros.messages.geometry_msgs import Vector3
 from compas_fab.backends.ros.messages.geometry_msgs import Quaternion
-from compas_fab.backends.ros.messages.shape_msgs import SolidPrimitive
-from compas_fab.backends.ros.messages.shape_msgs import Plane
-from compas_fab.backends.ros.messages.shape_msgs import Mesh
+from compas_fab.backends.ros.messages.geometry_msgs import Vector3
+from compas_fab.backends.ros.messages.object_recognition_msgs import ObjectType
+from compas_fab.backends.ros.messages.octomap_msgs import OctomapWithPose
 from compas_fab.backends.ros.messages.sensor_msgs import JointState
 from compas_fab.backends.ros.messages.sensor_msgs import MultiDOFJointState
+from compas_fab.backends.ros.messages.shape_msgs import Mesh
+from compas_fab.backends.ros.messages.shape_msgs import Plane
+from compas_fab.backends.ros.messages.shape_msgs import SolidPrimitive
+from compas_fab.backends.ros.messages.std_msgs import Header
+from compas_fab.backends.ros.messages.std_msgs import ROSmsg
 from compas_fab.backends.ros.messages.trajectory_msgs import JointTrajectory
 from compas_fab.backends.ros.messages.trajectory_msgs import MultiDOFJointTrajectory
-from compas_fab.backends.ros.messages.object_recognition_msgs import ObjectType
-from compas_fab.backends.ros.messages.octomap_msgs import OctomapWithPose
 
 
 class CollisionObject(ROSmsg):
@@ -255,23 +255,25 @@ class PlannerParams(ROSmsg):
     """
 
     def __init__(self, keys=None, values=None, descriptions=None):
-        self.keys = keys if keys else [] # parameter names (same size as values)
-        self.values = values if values else [] # parameter values (same size as keys)
-        self.descriptions = descriptions if descriptions else [] # parameter description (can be empty)
+        self.keys = keys or []                  # parameter names (same size as values)
+        self.values = values or []              # parameter values (same size as keys)
+        self.descriptions = descriptions or []  # parameter description (can be empty)
+
 
 class WorkspaceParameters(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/WorkspaceParameters.html
     """
-    def __init__(self, header=Header(), min_corner=Vector3(-1000,-1000,-1000), max_corner=Vector3(1000,1000,1000)):
+    def __init__(self, header=Header(), min_corner=Vector3(-1000, -1000, -1000), max_corner=Vector3(1000, 1000, 1000)):
         self.header = header
         self.min_corner = min_corner
         self.max_corner = max_corner
 
+
 class TrajectoryConstraints(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/TrajectoryConstraints.html
     """
     def __init__(self, constraints=None):
-        self.constraints = constraints if constraints else [] #Constraints[]
+        self.constraints = constraints or []  # Constraints[]
 
 
 class JointConstraint(ROSmsg):
@@ -291,21 +293,23 @@ def from_joint_constraint(cls, joint_constraint):
         c = joint_constraint
         return cls(c.joint_name, c.value, c.tolerance, c.tolerance, c.weight)
 
+
 class VisibilityConstraint(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/VisibilityConstraint.html
     """
     def __init__(self):
         raise NotImplementedError
 
+
 class BoundingVolume(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/BoundingVolume.html
     """
     def __init__(self, primitives=None, primitive_poses=None, meshes=None,
                  mesh_poses=None):
-        self.primitives = primitives if primitives else [] #shape_msgs/SolidPrimitive[]
-        self.primitive_poses = primitive_poses if primitive_poses else [] #geometry_msgs/Pose[]
-        self.meshes = meshes if meshes else [] #shape_msgs/Mesh[]
-        self.mesh_poses = mesh_poses if mesh_poses else [] #geometry_msgs/Pose[]
+        self.primitives = primitives or []            # shape_msgs/SolidPrimitive[]
+        self.primitive_poses = primitive_poses or []  # geometry_msgs/Pose[]
+        self.meshes = meshes or []                    # shape_msgs/Mesh[]
+        self.mesh_poses = mesh_poses or []            # geometry_msgs/Pose[]
 
     @classmethod
     def from_box(cls, box):
@@ -360,16 +364,17 @@ def from_bounding_volume(cls, bounding_volume):
         else:
             raise NotImplementedError
 
+
 class PositionConstraint(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/PositionConstraint.html
     """
     def __init__(self, header=None, link_name=None, target_point_offset=None,
                  constraint_region=None, weight=None):
-        self.header = header if header else Header()
-        self.link_name = link_name if link_name else ""
-        self.target_point_offset = target_point_offset if target_point_offset else Vector3(0.,0.,0.) # geometry_msgs/Vector3
-        self.constraint_region = constraint_region if constraint_region else BoundingVolume() # moveit_msgs/BoundingVolume
-        self.weight = float(weight) if weight else 1.
+        self.header = header or Header()
+        self.link_name = link_name or ""
+        self.target_point_offset = target_point_offset or Vector3(0., 0., 0.)  # geometry_msgs/Vector3
+        self.constraint_region = constraint_region or BoundingVolume()         # moveit_msgs/BoundingVolume
+        self.weight = float(weight) or 1.
 
     @classmethod
     def from_position_constraint(cls, header, position_constraint):
@@ -378,6 +383,7 @@ def from_position_constraint(cls, header, position_constraint):
         constraint_region = BoundingVolume.from_bounding_volume(position_constraint.bounding_volume)
         return cls(header, position_constraint.link_name, None, constraint_region, position_constraint.weight)
 
+
 class OrientationConstraint(ROSmsg):
     """http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/OrientationConstraint.html
     """
@@ -393,9 +399,9 @@ def __init__(self, header=None, orientation=None, link_name=None,
         the z-axis by an angle of 6.3 radians, whereas the z-axis can only change
         by 0.01.
         """
-        self.header = header if header else Header()
-        self.orientation = orientation or Quaternion()#geometry_msgs/Quaternion
-        self.link_name = link_name if link_name else ""
+        self.header = header or Header()
+        self.orientation = orientation or Quaternion()  # geometry_msgs/Quaternion
+        self.link_name = link_name or ""
         self.absolute_x_axis_tolerance = float(absolute_x_axis_tolerance)
         self.absolute_y_axis_tolerance = float(absolute_y_axis_tolerance)
         self.absolute_z_axis_tolerance = float(absolute_z_axis_tolerance)
@@ -449,7 +455,7 @@ def human_readable(self):
         return ''
 
 
-class  AllowedCollisionMatrix(ROSmsg):
+class AllowedCollisionMatrix(ROSmsg):
     """http://docs.ros.org/melodic/api/moveit_msgs/html/msg/AllowedCollisionMatrix.html
     """
     def __init__(self, entry_names=None, entry_values=None, default_entry_names=None, default_entry_values=None):
@@ -481,29 +487,16 @@ def __init__(self, name='', robot_state=None, robot_model_name='',
                  fixed_frame_transforms=None, allowed_collision_matrix=None,
                  link_padding=None, link_scale=None, object_colors=None, world=None,
                  is_diff=False):
-        self.name = name  # string
-        self.robot_state = robot_state or RobotState()  # moveit_msgs/RobotState
-        self.robot_model_name = robot_model_name  # string
+        self.name = name                                            # string
+        self.robot_state = robot_state or RobotState()              # moveit_msgs/RobotState
+        self.robot_model_name = robot_model_name                    # string
         self.fixed_frame_transforms = fixed_frame_transforms or []  # geometry_msgs/TransformStamped[]
         self.allowed_collision_matrix = allowed_collision_matrix or AllowedCollisionMatrix()
-        self.link_padding = link_padding or []  # moveit_msgs/LinkPadding[]
-        self.link_scale = link_scale or []  # moveit_msgs/LinkScale[]
-        self.object_colors = object_colors or []  # moveit_msgs/ObjectColor[]
-        self.world = world or PlanningSceneWorld()  # moveit_msgs/PlanningSceneWorld
-        self.is_diff = is_diff  # bool
-
-        """
-        SCENE_SETTINGS = 1
-        ROBOT_STATE=2
-        ROBOT_STATE_ATTACHED_OBJECTS=4
-        WORLD_OBJECT_NAMES=8
-        WORLD_OBJECT_GEOMETRY=16
-        OCTOMAP=32
-        TRANSFORMS=64
-        ALLOWED_COLLISION_MATRIX=128
-        LINK_PADDING_AND_SCALING=256
-        OBJECT_COLORS=512
-        """
+        self.link_padding = link_padding or []                      # moveit_msgs/LinkPadding[]
+        self.link_scale = link_scale or []                          # moveit_msgs/LinkScale[]
+        self.object_colors = object_colors or []                    # moveit_msgs/ObjectColor[]
+        self.world = world or PlanningSceneWorld()                  # moveit_msgs/PlanningSceneWorld
+        self.is_diff = is_diff                                      # bool
 
     @classmethod
     def from_msg(cls, msg):
@@ -515,3 +508,36 @@ def from_msg(cls, msg):
                    msg['fixed_frame_transforms'], allowed_collision_matrix,
                    msg['link_padding'], msg['link_scale'], msg['object_colors'],
                    world, msg['is_diff'])
+
+
+class ExecuteTrajectoryGoal(ROSmsg):
+    """http://docs.ros.org/kinetic/api/moveit_msgs/html/action/ExecuteTrajectory.html
+    """
+
+    def __init__(self, trajectory=None):
+        self.trajectory = trajectory or RobotTrajectory()
+
+
+class ExecuteTrajectoryFeedback(ROSmsg):
+    """http://docs.ros.org/kinetic/api/moveit_msgs/html/action/ExecuteTrajectory.html
+    """
+
+    def __init__(self, state=None):
+        self.state = state
+
+    @classmethod
+    def from_msg(cls, msg):
+        return cls(msg['state'])
+
+
+class ExecuteTrajectoryResult(ROSmsg):
+    """http://docs.ros.org/kinetic/api/moveit_msgs/html/action/ExecuteTrajectory.html
+    """
+
+    def __init__(self, error_code=None):
+        self.error_code = error_code or MoveItErrorCodes()  # moveit_msgs/MoveItErrorCodes
+
+    @classmethod
+    def from_msg(cls, msg):
+        error_code = MoveItErrorCodes.from_msg(msg['error_code'])
+        return cls(error_code)
