Fix for none constraints

Author: gonzalocasas

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -113,6 +113,27 @@ def dispose_planner(self):
         if hasattr(self, 'attached_collision_object_topic') and self.attached_collision_object_topic:
             self.attached_collision_object_topic.unadvertise()
 
+    def _convert_constraints_to_rosmsg(self, constraints, header):
+        """Convert COMPAS FAB constraints into ROS Messages."""
+        if not constraints:
+            return None
+
+        ros_constraints = Constraints()
+        for c in constraints:
+            if c.type == c.JOINT:
+                ros_constraints.joint_constraints.append(
+                    JointConstraint.from_joint_constraint(c))
+            elif c.type == c.POSITION:
+                ros_constraints.position_constraints.append(
+                    PositionConstraint.from_position_constraint(header, c))
+            elif c.type == c.ORIENTATION:
+                ros_constraints.orientation_constraints.append(
+                    OrientationConstraint.from_orientation_constraint(header, c))
+            else:
+                raise NotImplementedError
+
+        return ros_constraints
+
     # ==========================================================================
     # planning services
     # ==========================================================================
@@ -132,22 +153,8 @@ def inverse_kinematics_async(self, callback, errback, frame, base_link, group,
             for acm in attached_collision_meshes:
                 aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
                 start_state.attached_collision_objects.append(aco)
-        
-        # constraints
-        ik_constraints = Constraints()
-        for c in constraints:
-            if c.type == c.JOINT:
-                ik_constraints.joint_constraints.append(
-                    JointConstraint.from_joint_constraint(c))
-            elif c.type == c.POSITION:
-                ik_constraints.position_constraints.append(
-                    PositionConstraint.from_position_constraint(header, c))
-            elif c.type == c.ORIENTATION:
-                ik_constraints.orientation_constraints.append(
-                    OrientationConstraint.from_orientation_constraint(header, c))
-            else:
-                raise NotImplementedError
-        constraints = ik_constraints
+
+        constraints = self._convert_constraints_to_rosmsg(constraints, header)
 
         ik_request = PositionIKRequest(group_name=group,
                                        robot_state=start_state,