Convert constraints on cartesian and kinematic planners

Author: gonzalocasas

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -33,6 +33,7 @@
 from compas_fab.backends.ros.messages import PositionConstraint
 from compas_fab.backends.ros.messages import PositionIKRequest
 from compas_fab.backends.ros.messages import RobotState
+from compas_fab.backends.ros.messages import TrajectoryConstraints
 from compas_fab.backends.ros.planner_backend import PlannerBackend
 from compas_fab.backends.ros.planner_backend import ServiceDescription
 from compas_fab.robots import Configuration
@@ -206,6 +207,8 @@ def plan_cartesian_motion_async(self, callback, errback,
                 aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
                 start_state.attached_collision_objects.append(aco)
 
+        path_constraints = self._convert_constraints_to_rosmsg(path_constraints, header)
+
         request = dict(header=header,
                        start_state=start_state,
                        group_name=group,
@@ -264,38 +267,12 @@ def plan_motion_async(self, callback, errback,
                 aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
                 start_state.attached_collision_objects.append(aco)
 
-        # goal constraints
-        constraints = Constraints()
-        for c in goal_constraints:
-            if c.type == c.JOINT:
-                constraints.joint_constraints.append(
-                    JointConstraint.from_joint_constraint(c))
-            elif c.type == c.POSITION:
-                constraints.position_constraints.append(
-                    PositionConstraint.from_position_constraint(header, c))
-            elif c.type == c.ORIENTATION:
-                constraints.orientation_constraints.append(
-                    OrientationConstraint.from_orientation_constraint(header, c))
-            else:
-                raise NotImplementedError
-        goal_constraints = [constraints]
-
-        # path constraints
-        if path_constraints:
-            constraints = Constraints()
-            for c in path_constraints:
-                if c.type == c.JOINT:
-                    constraints.joint_constraints.append(
-                        JointConstraint.from_joint_constraint(c))
-                elif c.type == c.POSITION:
-                    constraints.position_constraints.append(
-                        PositionConstraint.from_position_constraint(header, c))
-                elif c.type == c.ORIENTATION:
-                    constraints.orientation_constraints.append(
-                        OrientationConstraint.from_orientation_constraint(header, c))
-                else:
-                    raise NotImplementedError
-            path_constraints = constraints
+        # convert constraints
+        goal_constraints = [self._convert_constraints_to_rosmsg(goal_constraints, header)]
+        path_constraints = self._convert_constraints_to_rosmsg(path_constraints, header)
+
+        if trajectory_constraints is not None:
+            trajectory_constraints = TrajectoryConstraints(constraints=self._convert_constraints_to_rosmsg(path_constraints, header))
 
         request = dict(start_state=start_state,
                        goal_constraints=goal_constraints,