Merge pull request #345 from compas-dev/fix-mesh-poses-noetic

fix: pose of collision meshes was ignored in ROS Noetic

Author: gonzalocasas

CHANGELOG.rst

@@ -26,6 +26,7 @@ Unreleased
 * Fixed PyBullet loading of meshes.
 * Fixed missing flag in reset planning scene call.
 * Fixed issue on cartesian and kinematic planning when model contains passive joints.
+* Fixed pose of collision mesh in ROS Noetic being ignored.
 
 **Deprecated**
 

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

@@ -55,5 +55,5 @@ def add_attached_collision_mesh_async(self, callback, errback, attached_collisio
         aco.object.operation = CollisionObject.ADD
         robot_state = RobotState(attached_collision_objects=[aco], is_diff=True)
         scene = PlanningScene(robot_state=robot_state, is_diff=True)
-        request = dict(scene=scene)
+        request = scene.to_request(self.ros_client.ros_distro)
         self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)

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

@@ -53,5 +53,5 @@ def add_collision_mesh_async(self, callback, errback, collision_mesh):
         co.operation = CollisionObject.ADD
         world = PlanningSceneWorld(collision_objects=[co])
         scene = PlanningScene(world=world, is_diff=True)
-        request = dict(scene=scene)
+        request = scene.to_request(self.ros_client.ros_distro)
         self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)

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

@@ -53,5 +53,5 @@ def append_collision_mesh_async(self, callback, errback, collision_mesh):
         co.operation = CollisionObject.APPEND
         world = PlanningSceneWorld(collision_objects=[co])
         scene = PlanningScene(world=world, is_diff=True)
-        request = dict(scene=scene)
+        request = scene.to_request(self.ros_client.ros_distro)
         self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)

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

@@ -55,5 +55,5 @@ def remove_attached_collision_mesh_async(self, callback, errback, id):
         aco.object.operation = CollisionObject.REMOVE
         robot_state = RobotState(attached_collision_objects=[aco], is_diff=True)
         scene = PlanningScene(robot_state=robot_state, is_diff=True)
-        request = dict(scene=scene)
+        request = scene.to_request(self.ros_client.ros_distro)
         self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)

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

@@ -54,5 +54,5 @@ def remove_collision_mesh_async(self, callback, errback, id):
         co.operation = CollisionObject.REMOVE
         world = PlanningSceneWorld(collision_objects=[co])
         scene = PlanningScene(world=world, is_diff=True)
-        request = dict(scene=scene)
+        request = scene.to_request(self.ros_client.ros_distro)
         self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)

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

@@ -51,5 +51,5 @@ def reset_planning_scene_async(self, callback, errback):
             collision_object.object['operation'] = CollisionObject.REMOVE
         scene.is_diff = True
         scene.robot_state.is_diff = True
-        request = dict(scene=scene)
+        request = scene.to_request(self.ros_client.ros_distro)
         self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)

src/compas_fab/backends/ros/client.py

@@ -157,7 +157,7 @@ def __exit__(self, *args):
     def ros_distro(self):
         """Retrieves the ROS version to which the client is connected (eg. kinetic)"""
         if not self._ros_distro:
-            self._ros_distro = Param(self, '/rosdistro').get(timeout=1)
+            self._ros_distro = Param(self, '/rosdistro').get(timeout=1).strip()
 
         return self._ros_distro
 

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

@@ -7,6 +7,7 @@
 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.ros_releases import RosDistro
 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
@@ -38,6 +39,8 @@ def __init__(self, header=None, id="collision_obj", type=None,
         self.header = header or Header()  # a header, used for interpreting the poses
         self.id = id  # the id of the object (name used in MoveIt)
         self.type = type or ObjectType()  # The object type in a database of known objects
+        self.pose = pose or Pose()  # currently not actively used in FAB, but needed to be present otherwise ROS Noetic complains about empty quaternion
+
         # solid geometric primitives
         self.primitives = primitives or []
         self.primitive_poses = primitive_poses or []
@@ -59,6 +62,7 @@ def from_collision_mesh(cls, collision_mesh):
         kwargs['id'] = collision_mesh.id
         kwargs['meshes'] = [Mesh.from_mesh(collision_mesh.mesh)]
         kwargs['mesh_poses'] = [Pose.from_frame(collision_mesh.frame)]
+        kwargs['pose'] = Pose()
 
         return cls(**kwargs)
 
@@ -69,6 +73,7 @@ def from_msg(cls, msg):
         kwargs['header'] = Header.from_msg(msg['header'])
         kwargs['id'] = msg['id']
         kwargs['type'] = ObjectType.from_msg(msg['type'])
+        kwargs['pose'] = Pose.from_msg(msg['pose']) if 'pose' in msg else Pose()
 
         kwargs['primitives'] = [SolidPrimitive.from_msg(i) for i in msg['primitives']]
         kwargs['primitive_poses'] = [Pose.from_msg(i) for i in msg['primitive_poses']]
@@ -555,6 +560,20 @@ def __init__(self, name='', robot_state=None, robot_model_name='',
         self.world = world or PlanningSceneWorld()                  # moveit_msgs/PlanningSceneWorld
         self.is_diff = is_diff                                      # bool
 
+    def to_request(self, ros_distro):
+        self.filter_fields_for_distro(ros_distro)
+        return dict(scene=self)
+
+    def filter_fields_for_distro(self, ros_distro):
+        """To maintain backwards compatibility with older ROS distros,
+        we need to make sure newly added fields are removed from the request."""
+        # Remove the field `pose` for distros older than NOETIC
+        if ros_distro in (RosDistro.KINETIC, RosDistro.MELODIC):
+            for aco in self.robot_state.attached_collision_objects:
+                del aco.object.pose
+            for co in self.world.collision_objects:
+                del co.pose
+
     @classmethod
     def from_msg(cls, msg):
         robot_state = RobotState.from_msg(msg['robot_state'])