Add support for compas_fab.robots.JointTrajectory on ROS trajectory follower

Author: gonzalocasas

src/compas_fab/backends/ros/client.py

@@ -10,8 +10,8 @@
 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
-from compas_fab.backends.ros.messages import JointTrajectoryPoint
+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 Time
 from compas_fab.backends.ros.planner_backend_moveit import MoveItPlanner
 from compas_fab.backends.tasks import CancellableTask
@@ -81,7 +81,6 @@ def __init__(self, host='localhost', port=9090, is_secure=False, planner_backend
         planner_backend_type = PLANNER_BACKENDS[planner_backend]
         self.__class__ = type('RosClient_' + planner_backend_type.__name__, (planner_backend_type, RosClient), {})
 
-
     def __enter__(self):
         self.run()
         self.connect()
@@ -227,7 +226,7 @@ def remove_attached_collision_mesh(self, id):
     def get_configuration(self):
         pass
 
-    def follow_configurations(self, callback, joint_names, configurations, timesteps, timeout=None):
+    def follow_configurations(self, callback, joint_names, configurations, timesteps, timeout=60000):
 
         if len(configurations) != len(timesteps):
             raise ValueError("%d configurations must have %d timesteps, but %d given." % (
@@ -239,21 +238,44 @@ def follow_configurations(self, callback, joint_names, configurations, timesteps
         points = []
         num_joints = len(configurations[0].values)
         for config, time in zip(configurations, timesteps):
-            pt = JointTrajectoryPoint(positions=config.values, velocities=[
+            pt = RosMsgJointTrajectoryPoint(positions=config.values, velocities=[
                                       0]*num_joints, time_from_start=Time(secs=(time)))
             points.append(pt)
 
-        joint_trajectory = JointTrajectory(
+        joint_trajectory = RosMsgJointTrajectory(
             Header(), joint_names, points)  # specify header necessary?
-        return self.follow_joint_trajectory(callback, joint_trajectory, timeout)
+        return self.follow_joint_trajectory(joint_trajectory=joint_trajectory, callback=callback, timeout=timeout)
+
+    def _convert_to_ros_trajectory(self, joint_trajectory):
+        """Converts a ``compas_fab.robots.JointTrajectory`` into a ROS Msg joint trajectory."""
+
+        # For backwards-compatibility, accept ROS Msg directly as well and simply do not modify
+        if isinstance(joint_trajectory, RosMsgJointTrajectory):
+            return joint_trajectory
+
+        trajectory = RosMsgJointTrajectory()
+        trajectory.joint_names = joint_trajectory.joint_names
+
+        for point in joint_trajectory.points:
+            ros_point = RosMsgJointTrajectoryPoint(
+                positions=point.positions,
+                velocities=point.velocities,
+                accelerations=point.accelerations,
+                effort=point.effort,
+                time_from_start=Time(
+                    point.time_from_start.secs, point.time_from_start.nsecs),
+            )
+            trajectory.points.append(ros_point)
+
+        return trajectory
 
     def follow_joint_trajectory(self, joint_trajectory, action_name='/joint_trajectory_action', callback=None, errback=None, feedback_callback=None, timeout=60000):
         """Follow the joint trajectory as computed by MoveIt planner.
 
         Parameters
         ----------
-        joint_trajectory : JointTrajectory
-            Joint trajectory message as computed by MoveIt planner
+        joint_trajectory : :class:`compas_fab.robots.JointTrajectory`
+            Instance of joint trajectory. Note: for backwards compatibility, this supports a ROS Msg being passed as well.
         action_name : string
             ROS action name, defaults to ``/joint_trajectory_action`` but some drivers need ``/follow_joint_trajectory``.
         callback : callable
@@ -273,8 +295,8 @@ def follow_joint_trajectory(self, joint_trajectory, action_name='/joint_trajecto
             An instance of a cancellable tasks.
         """
 
-        trajectory_goal = FollowJointTrajectoryGoal(
-            trajectory=joint_trajectory)
+        joint_trajectory = self._convert_to_ros_trajectory(joint_trajectory)
+        trajectory_goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory)
 
         def handle_result(msg, client):
             result = FollowJointTrajectoryResult.from_msg(msg)
@@ -302,4 +324,3 @@ def handle_failure(error):
         goal.send(timeout=timeout)
 
         return CancellableRosAction(goal)
-

src/compas_fab/backends/ros/messages/control_msgs.py

@@ -12,52 +12,52 @@ class JointTolerance(ROSmsg):
     """
     def __init__(self, name="", position=0., velocity=0., acceleration=0.):
         self.name = name
-        self.position = position # in radians or meters (for a revolute or prismatic joint, respectively)
-        self.velocity = velocity  # in rad/sec or m/sec
-        self.acceleration = acceleration # in rad/sec^2 or m/sec^2
+        self.position = position          # in radians or meters (for a revolute or prismatic joint, respectively)
+        self.velocity = velocity          # in rad/sec or m/sec
+        self.acceleration = acceleration  # in rad/sec^2 or m/sec^2
 
 
 class FollowJointTrajectoryGoal(ROSmsg):
     """http://docs.ros.org/fuerte/api/control_msgs/html/msg/FollowJointTrajectoryGoal.html
     """
 
-    def __init__(self, trajectory=None, path_tolerance=None, 
+    def __init__(self, trajectory=None, path_tolerance=None,
                  goal_tolerance=None, goal_time_tolerance=None):
-        self.trajectory = trajectory if trajectory else JointTrajectory() #trajectory_msgs/JointTrajectory 
-        self.path_tolerance = path_tolerance if path_tolerance else []#control_msgs/JointTolerance[] 
-        self.goal_tolerance = goal_tolerance if goal_tolerance else [] #control_msgs/JointTolerance[] 
-        self.goal_time_tolerance = goal_time_tolerance if goal_time_tolerance else Time(secs=1.)
+        self.trajectory = trajectory or JointTrajectory()  # trajectory_msgs/JointTrajectory
+        self.path_tolerance = path_tolerance or []         # control_msgs/JointTolerance[]
+        self.goal_tolerance = goal_tolerance or []         # control_msgs/JointTolerance[]
+        self.goal_time_tolerance = goal_time_tolerance or Time(secs=1.)
 
 
 class FollowJointTrajectoryActionGoal(ROSmsg):
     """http://docs.ros.org/fuerte/api/control_msgs/html/msg/FollowJointTrajectoryActionGoal.html
     """
 
     def __init__(self, header=None, goal_id=None, goal=None):
-        self.header = header if header else Header()
-        self.goal_id = goal_id if goal_id else GoalID() #actionlib_msgs/GoalID goal_id
-        self.goal = goal if goal else FollowJointTrajectoryGoal() # FollowJointTrajectoryGoal goal
+        self.header = header or Header()
+        self.goal_id = goal_id or GoalID()  # actionlib_msgs/GoalID goal_id
+        self.goal = goal or FollowJointTrajectoryGoal()  # FollowJointTrajectoryGoal goal
 
 
 class FollowJointTrajectoryFeedback(ROSmsg):
     """http://docs.ros.org/fuerte/api/control_msgs/html/msg/FollowJointTrajectoryFeedback.html
     """
     def __init__(self, header=None, joint_names=None, desired=None, actual=None,
                  error=None):
-        self.header = header if header else Header()
-        self.joint_names = joint_names if joint_names else []
-        self.desired = desired if desired else JointTrajectoryPoint()
-        self.actual = actual if actual else JointTrajectoryPoint()
-        self.error = error if error else JointTrajectoryPoint()
+        self.header = header or Header()
+        self.joint_names = joint_names or []
+        self.desired = desired or JointTrajectoryPoint()
+        self.actual = actual or JointTrajectoryPoint()
+        self.error = error or JointTrajectoryPoint()
 
 
 class FollowJointTrajectoryActionFeedback(ROSmsg):
     """http://docs.ros.org/fuerte/api/control_msgs/html/msg/FollowJointTrajectoryActionFeedback.html
     """
     def __init__(self, header=None, status=None, feedback=None):
-        self.header = header if header else Header()
-        self.status = status if status else GoalStatus()
-        self.feedback = feedback if feedback else FollowJointTrajectoryFeedback()
+        self.header = header or Header()
+        self.status = status or GoalStatus()
+        self.feedback = feedback or FollowJointTrajectoryFeedback()
 
 
 class FollowJointTrajectoryResult(ROSmsg):
@@ -74,7 +74,7 @@ class FollowJointTrajectoryResult(ROSmsg):
     def __init__(self, error_code=0, error_string=""):
         self.error_code = error_code
         self.error_string = error_string
-    
+
     @classmethod
     def from_msg(cls, msg):
         error_code = msg['error_code']
@@ -93,14 +93,13 @@ class FollowJointTrajectoryActionResult(ROSmsg):
     """http://docs.ros.org/fuerte/api/control_msgs/html/msg/FollowJointTrajectoryActionResult.html
     """
     def __init__(self, header=None, status=None, result=None):
-        self.header = header if header else Header()
-        self.status = status if status else GoalStatus()
-        self.result = result if result else FollowJointTrajectoryResult()
-    
+        self.header = header or Header()
+        self.status = status or GoalStatus()
+        self.result = result or FollowJointTrajectoryResult()
+
     @classmethod
     def from_msg(cls, msg):
         header = Header.from_msg(msg['header'])
         status = GoalStatus.from_msg(msg['status'])
         result = FollowJointTrajectoryResult.from_msg(msg['result'])
         return cls(header, status, result)
-

src/compas_fab/backends/ros/messages/trajectory_msgs.py

@@ -4,20 +4,17 @@
 from .std_msgs import Header
 from .std_msgs import Time
 
-from .geometry_msgs import Transform
-from .geometry_msgs import Twist
-
 
 class JointTrajectoryPoint(ROSmsg):
     """http://docs.ros.org/kinetic/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html
     """
 
     def __init__(self, positions=None, velocities=None, accelerations=None, effort=None, time_from_start=None):
-        self.positions = positions if positions else []
-        self.velocities = velocities if velocities else []
-        self.accelerations = accelerations if accelerations else []
-        self.effort = effort if effort else []
-        self.time_from_start = time_from_start if time_from_start else Time()
+        self.positions = positions or []
+        self.velocities = velocities or []
+        self.accelerations = accelerations or []
+        self.effort = effort or []
+        self.time_from_start = time_from_start or Time()
         # TODO: check if we need to enter zeros to all
 
     @classmethod
@@ -40,9 +37,9 @@ class JointTrajectory(ROSmsg):
     """
 
     def __init__(self, header=None, joint_names=None, points=None):
-        self.header = header if header else Header()
-        self.joint_names = joint_names if joint_names else []
-        self.points = points if points else []
+        self.header = header or Header()
+        self.joint_names = joint_names or []
+        self.points = points or []
 
     @classmethod
     def from_msg(cls, msg):
@@ -57,20 +54,20 @@ class MultiDOFJointTrajectoryPoint(ROSmsg):
     """
 
     def __init__(self, transforms=None, velocities=None, accelerations=None, time_from_start=None):
-        self.transforms = transforms if transforms else []  # geometry_msgs/Transform[]
-        self.velocities = velocities if velocities else []  # geometry_msgs/Twist[]
-        self.accelerations = accelerations if accelerations else []  # geometry_msgs/Twist[]
-        self.time_from_start = time_from_start if time_from_start else Time()
+        self.transforms = transforms or []        # geometry_msgs/Transform[]
+        self.velocities = velocities or []        # geometry_msgs/Twist[]
+        self.accelerations = accelerations or []  # geometry_msgs/Twist[]
+        self.time_from_start = time_from_start or Time()
 
 
 class MultiDOFJointTrajectory(ROSmsg):
     """http://docs.ros.org/kinetic/api/trajectory_msgs/html/msg/MultiDOFJointTrajectory.html
     """
 
     def __init__(self, header=None, joint_names=None, points=None):
-        self.header = header if header else Header()
-        self.joint_names = joint_names if joint_names else []
-        self.points = points if points else []
+        self.header = header or Header()
+        self.joint_names = joint_names or []
+        self.points = points or []
 
     @classmethod
     def from_msg(cls, msg):

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -151,7 +151,7 @@ def forward_kinematics_async(self, callback, errback, joint_positions, base_link
     def plan_cartesian_motion_async(self, callback, errback, frames, base_link,
                                     ee_link, group, joint_names, joint_types,
                                     start_configuration, max_step, jump_threshold,
-                                    avoid_collisions, path_constraints, 
+                                    avoid_collisions, path_constraints,
                                     attached_collision_meshes):
         """Asynchronous handler of MoveIt cartesian motion planner service."""
         header = Header(frame_id=base_link)
@@ -179,6 +179,7 @@ def convert_to_trajectory(response):
             trajectory = JointTrajectory()
             trajectory.source_message = response
             trajectory.fraction = response.fraction
+            trajectory.joint_names = response.solution.joint_trajectory.joint_names
             trajectory.points = convert_trajectory_points(response.solution.joint_trajectory.points, joint_types)
             trajectory.start_configuration = Configuration(response.start_state.joint_state.position, start_configuration.types)
 
@@ -260,6 +261,7 @@ def convert_to_trajectory(response):
             trajectory = JointTrajectory()
             trajectory.source_message = response
             trajectory.fraction = 1.
+            trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
             trajectory.points = convert_trajectory_points(response.trajectory.joint_trajectory.points, joint_types)
             trajectory.start_configuration = Configuration(response.trajectory_start.joint_state.position, start_configuration.types)
             trajectory.planning_time = response.planning_time

src/compas_fab/robots/trajectory.py

@@ -140,18 +140,21 @@ class JointTrajectory(Trajectory):
 
     Attributes
     ----------
-    points: :obj:`list` of :class:`JointTrajectoryPoint`
+    trajectory_points: :obj:`list` of :class:`JointTrajectoryPoint`
         List of points composing the trajectory.
+    joint_names: :obj:`list` of :obj:`str`
+        List of joint names of the trajectory.
     start_configuration: :class:`Configuration`
         Start configuration for the trajectory.
     fraction: float
         Indicates the percentage of requested trajectory that was calcuted,
         e.g. ``1`` means the full trajectory was found.
     """
 
-    def __init__(self, trajectory_points=[], start_configuration=None, fraction=None):
+    def __init__(self, trajectory_points=[], joint_names=[], start_configuration=None, fraction=None):
         super(Trajectory, self).__init__()
         self.points = trajectory_points
+        self.joint_names = joint_names
         self.start_configuration = start_configuration
         self.fraction = fraction
 
@@ -181,15 +184,18 @@ def to_data(self):
     @property
     def data(self):
         """:obj:`dict` : The data representing the trajectory."""
-        return {
-            'points': [p.to_data() for p in self.points],
-            'start_configuration': self.start_configuration.to_data(),
-            'fraction': self.fraction,
-        }
+        data_obj = {}
+        data_obj['points'] = [p.to_data() for p in self.points]
+        data_obj['joint_names'] = self.joint_names or []
+        data_obj['start_configuration'] = self.start_configuration.to_data()
+        data_obj['fraction'] = self.fraction
+
+        return data_obj
 
     @data.setter
     def data(self, data):
         self.points = list(map(JointTrajectoryPoint.from_data, data.get('points') or []))
+        self.joint_names = data.get('joint_names', [])
         self.start_configuration = Configuration.from_data(data.get('start_configuration'))
         self.fraction = data.get('fraction')