fix #382: pre-noetic support for ik and planning features using MoveIt with an attached tool

Author: gonzalocasas

src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py

@@ -109,13 +109,15 @@ def inverse_kinematics_async(
             name=start_configuration.joint_names, position=start_configuration.joint_values, header=header
         )
         start_state = RobotState(joint_state, MultiDOFJointState(header=header))
-        start_state.filter_fields_for_distro(self.ros_client.ros_distro)
 
         if options.get("attached_collision_meshes"):
             for acm in options["attached_collision_meshes"]:
                 aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
                 start_state.attached_collision_objects.append(aco)
 
+        # Filter needs to happend after all objects have been added
+        start_state.filter_fields_for_distro(self.ros_client.ros_distro)
+
         constraints = convert_constraints_to_rosmsg(options.get("constraints"), header)
 
         timeout_in_secs = options.get("timeout", 2)

src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py

@@ -113,13 +113,15 @@ def plan_cartesian_motion_async(
             header=header, name=start_configuration.joint_names, position=start_configuration.joint_values
         )
         start_state = RobotState(joint_state, MultiDOFJointState(header=header), is_diff=True)
-        start_state.filter_fields_for_distro(self.ros_client.ros_distro)
 
         if options.get("attached_collision_meshes"):
             for acm in options["attached_collision_meshes"]:
                 aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
                 start_state.attached_collision_objects.append(aco)
 
+        # Filter needs to happend after all objects have been added
+        start_state.filter_fields_for_distro(self.ros_client.ros_distro)
+
         path_constraints = convert_constraints_to_rosmsg(options.get("path_constraints"), header)
 
         request = dict(

src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py

@@ -110,13 +110,15 @@ def plan_motion_async(
             header=header, name=start_configuration.joint_names, position=start_configuration.joint_values
         )
         start_state = RobotState(joint_state, MultiDOFJointState(header=header), is_diff=True)
-        start_state.filter_fields_for_distro(self.ros_client.ros_distro)
 
         if options.get("attached_collision_meshes"):
             for acm in options["attached_collision_meshes"]:
                 aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
                 start_state.attached_collision_objects.append(aco)
 
+        # Filter needs to happend after all objects have been added
+        start_state.filter_fields_for_distro(self.ros_client.ros_distro)
+
         # convert constraints
         goal_constraints = [convert_constraints_to_rosmsg(goal_constraints, header)]
         path_constraints = convert_constraints_to_rosmsg(options.get("path_constraints"), header)