Diff with hash in set_robot_cell

docs/examples/03_backends_ros/files/01_ros_set_robot_cell.py

@@ -42,3 +42,21 @@
     # If you are running ROS with UI, you should see a cone in the PyBullet world
     # and the floor should be gone
     input("Press Enter to continue...")
+
+    # =========
+    # Example 3
+    # =========
+
+    # Add the floor mesh to this robot cell
+    # The `cone` rigid body is also in the robot cell
+    robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh)
+
+    # Calling `set_robot_cell` again will update the robot cell in the planner
+    # Behind the scene, compas_fab will compare the new robot cell with the previous one
+    # Because the cone rigid body is identical to the previous one, the `cone` rigid body
+    # will not be sent again to the PyBullet backend. Only the `floor` rigid body will be sent.
+    result = planner.set_robot_cell(robot_cell)
+    print(result)
+
+    # If you are running ROS with UI, you should see the floor and the cone in the PyBullet world
+    input("Press Enter to continue...")

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

@@ -9,14 +9,15 @@
 from compas_fab.backends.ros.messages import ApplyPlanningSceneRequest
 from compas_fab.backends.ros.messages import ApplyPlanningSceneResponse
 from compas_fab.backends.ros.messages import CollisionObject
-from compas_fab.backends.ros.messages import AttachedCollisionObject
+from compas_fab.backends.ros.messages import CollisionMesh
 from compas_fab.backends.ros.messages import PlanningScene
 from compas_fab.backends.ros.messages import PlanningSceneWorld
 from compas_fab.backends.ros.messages.geometry_msgs import Header
 from compas_fab.backends.ros.messages.geometry_msgs import Pose
 from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.messages.shape_msgs import Mesh
 from compas_fab.backends.ros.service_description import ServiceDescription
+from compas_robots.model import LinkGeometry
 
 if not IPY:
     from typing import TYPE_CHECKING
@@ -69,66 +70,175 @@ def set_robot_cell_async(self, callback, errback, robot_cell, robot_cell_state=N
         # type: (Callable, Callable, RobotCell, Optional[RobotCellState], Optional[Dict]) -> None
         """Pass the models in the robot cell to the planning client.
 
-        The client keeps the robot cell models in memory and uses them for planning.
-        Calling this method will override the previous robot cell in the client.
+        MoveIt keeps all the robot cell models in memory and uses them for planning.
+
+        Calling this method will override the previous robot cell passed to the client.
+        Setting the robot cell is a slow operation, as it involves sending the
+        geometry of the robot cell to the planning client.
         It should be called only if the robot cell models have changed.
 
+        This function is responsible for creating moveit_msgs/CollisionObject messages
+        for each RigidBody in the robot cell with the ADD operation.
+
+        Tools are also considered as MoveIt CollisionsObjects in the same way.
+        Because MoveIt backend does not support kinematic tools (with movable parts),
+        they are treated as static objects fixed at their zero configuration.
         """
         planner = self  # type: MoveItPlanner  # noqa: F841
-        client = self.client  # type: PyBulletClient
+        client = planner.client  # type: PyBulletClient
         robot = client.robot
 
         assert robot_cell, "Robot cell should not be None"
         assert robot_cell.robot, "Robot cell should have a robot"
 
-        # First implementation test
-        # We remove all objects and add them again, the diff is left to the client
+        last_planning_scene_world = planner._last_planning_scene_world
 
-        # NOTE: Diff are all false for now. In the future we can generate actual diff here
-        # to reduce already-existing geometry being sent to over ROS
+        # Retrieve the previous planning scene to keep the allowed collision matrix
+        # previous_planning_scene = planner.get_planning_scene()  # type: PlanningScene
+        # print(previous_planning_scene.allowed_collision_matrix)
 
-        # Add the rigid bodies to the planning scene
-        collision_objects = {}
-        for rigid_body_id, rigid_body in robot_cell.rigid_body_models.items():
+        # `name` == '(noname)+'
+        # `object_colors`, `fixed_frame_transformation`, `link_padding` and `link_scale` are empty == []
+        # `robot` == [{'enabled': [True]}]
+
+        # NOTE: This function will only create CollisionObjects (not attached),
+        # The `set_robot_cell_state`` is responsible of the creation of AttachedCollisionObjects.
 
-            kwargs = {}
-            # The frame_id needs be the root name of the robot
-            kwargs["header"] = Header(frame_id=robot.root_name)
-            # id is the identifier of the rigid body
-            kwargs["id"] = rigid_body_id
-            # List of `compas_fab.backends.ros.messages.shape_msgs.Meshes`
-            kwargs["meshes"] = [Mesh.from_mesh(m) for m in rigid_body.collision_meshes]
-            # List of `compas_fab.backends.ros.messages.geometry_msgs.Pose`
-            kwargs["mesh_poses"] = [Pose() for _ in rigid_body.collision_meshes]
-            kwargs["operation"] = CollisionObject.ADD
-            if robot_cell_state:
-                rigid_body_frame = robot_cell_state.rigid_body_states[rigid_body_id].frame
-                kwargs["pose"] = Pose.from_frame(rigid_body_frame)
-            else:
-                kwargs["pose"] = Pose()
-
-            co = CollisionObject(**kwargs)
-            collision_objects[rigid_body_id] = co
-
-        # Add attached collision objects to the robot_state
-        attached_collision_objects = {}
-        if robot_cell_state:
-            for rigid_body_id in robot_cell_state.get_attached_rigid_body_ids():
-                rigid_body_state = robot_cell_state.rigid_body_states[rigid_body_id]
-                collision_object = collision_objects[rigid_body_id]
-
-                kwargs = {}
-                kwargs["link_name"] = rigid_body_state.attached_to_link
-                kwargs["object"] = collision_object
-                kwargs["touch_links"] = rigid_body_state.touch_links
-                kwargs["weight"] = self.ATTACHED_OBJECTS_WEIGHT
-
-                collision_object.pose = Pose.from_frame(rigid_body_state.attachment_frame)
-                attached_collision_objects[rigid_body_id] = AttachedCollisionObject(**kwargs)
-        robot_state = RobotState(attached_collision_objects=list(attached_collision_objects.values()), is_diff=False)
-
-        world = PlanningSceneWorld(collision_objects=list(collision_objects.values()))
-        scene = PlanningScene(robot_state=robot_state, world=world, is_diff=False)
+        collision_objects = []
+
+        # Remove rigid bodies from the previous planning scene that are not in the new robot cell
+        for rigid_body_id in list(planner._current_rigid_body_hashes):
+            if rigid_body_id not in robot_cell.rigid_body_models:
+                co = CollisionObject(
+                    id=rigid_body_id,
+                    operation=CollisionObject.REMOVE,
+                )
+                collision_objects.append(co)
+                # Remove the hash from the dictionary
+                del planner._current_rigid_body_hashes[rigid_body_id]
+                print("Rigid body {} removed from the planning scene".format(rigid_body_id))
+
+        # Add rigid bodies to the planning scene
+        # Skip those that are already in the planning scene
+
+        for rigid_body_id, rigid_body in robot_cell.rigid_body_models.items():
+            rigid_body_hash = rigid_body.sha256()
+            # Compare the hash of the rigid body with the previous one
+            if rigid_body_id in planner._current_rigid_body_hashes:
+                if rigid_body_hash == planner._current_rigid_body_hashes[rigid_body_id]:
+                    print("Rigid body {} skipped because is already in the planning scene".format(rigid_body_id))
+                    continue
+
+            # Create new CollisionObject to be passed to backend
+            ros_meshes = [Mesh.from_mesh(m) for m in rigid_body.collision_meshes]
+            collision_object = CollisionObject(
+                # Header is robot.root_name when the object is not attached
+                header=Header(frame_id=robot.root_name),
+                # The identifier of the rigid body, matches with the robot_cell rigid_body_id
+                id=rigid_body_id,
+                # List of `compas_fab.backends.ros.messages.shape_msgs.Meshes`
+                meshes=ros_meshes,
+                # List of Pose, one for each mesh
+                # NOTE: We leave the `mesh_poses` at origin because it cannot be moved later
+                mesh_poses=[Pose() for _ in ros_meshes],
+                # `pose` can be modified later, at the moment it is not set.
+                pose=Pose(),
+                # Operation is always ADD
+                operation=CollisionObject.ADD,
+            )
+            collision_objects.append(collision_object)
+            print("Rigid body {} added to the planning scene".format(rigid_body_id))
+
+            # Update the hash of the rigid body
+            planner._current_rigid_body_hashes[rigid_body_id] = rigid_body_hash
+
+        # # Add tools to the planning scene
+        # for tool_id, tool_model in robot_cell.tool_models.items():
+        #     # For each tool, iterate through its links, each link that has geometry(s) will create one collision object
+        #     # They are named as "[tool_name]_[link_name]_collision"
+        #     # The naming convention here is important, as we are planning to love the links later for a kinematic tool.
+        #     for link in tool_model.iter_links():
+        #         collision_meshes = []
+        #         # NOTE: There can be multiple Collision objects in a link, we flatten them to
+        #         for collision in link.collision:
+        #             collision_meshes += list(LinkGeometry._get_item_meshes(collision))
+        #         # For each mesh in the link, create a CollisionMesh
+        #         kwargs = {}
+        #         # The frame_id needs be the root name of the robot
+        #         kwargs["header"] = Header(frame_id=robot.root_name)
+        #         # id is the identifier of the rigid body
+        #         # In theory the `tool_id` from the tool_model dictionary is the same as the `tool_model.name`.
+        #         tool_collision_mesh_name = "{}_{}_collision".format(tool_id, link.name)
+        #         kwargs["id"] = tool_collision_mesh_name
+        #         # List of `compas_fab.backends.ros.messages.shape_msgs.Meshes`
+        #         kwargs["meshes"] = [Mesh.from_mesh(m) for m in collision_meshes]
+        #         # List of `compas_fab.backends.ros.messages.geometry_msgs.Pose`, individual pose for the links
+        #         # Pose will be set by set_robot_cell_state later
+        #         kwargs["mesh_poses"] = [Pose() for _ in collision_meshes]
+        #         kwargs["operation"] = CollisionObject.ADD
+        #         # Attachment pose for the tool to the attached_link
+        #         # Pose will be set by set_robot_cell_state later
+        #         kwargs["pose"] = Pose()
+        #         co = CollisionObject(**kwargs)
+        #         collision_objects[tool_collision_mesh_name] = co
+
+        world = PlanningSceneWorld(collision_objects=collision_objects)
+        # NOTE: There is no need to update or change the Allowed Collision Matrix here
+        # By setting `is_diff` to True, the allowed collision matrix for the robot will
+        # not be overridden.
+        scene = PlanningScene(world=world, is_diff=True)
 
         request = scene.to_request(client.ros_distro)
         self.APPLY_PLANNING_SCENE(client, request, callback, errback)
+
+
+# The allowed_collision_matrix is a dictionary with a structure similar to the following:
+# {
+#     "entry_names": [
+#         "base_link_inertia",
+#         "forearm_link",
+#         "shoulder_link",
+#         "upper_arm_link",
+#         "wrist_1_link",
+#         "wrist_2_link",
+#         "wrist_3_link",
+#     ],
+#     "entry_values": [
+#         {"enabled": [False, False, True, True, True, False, False]},
+#         {"enabled": [False, False, False, True, True, False, False]},
+#         {"enabled": [True, False, False, True, True, True, False]},
+#         {"enabled": [True, True, True, False, False, False, False]},
+#         {"enabled": [True, True, True, False, False, True, True]},
+#         {"enabled": [False, False, True, False, True, False, True]},
+#         {"enabled": [False, False, False, False, True, True, False]},
+#     ],
+#     "default_entry_names": [],
+#     "default_entry_values": [],
+# }
+
+# The robot_state is a RobotState object with a structure similar to the following:
+# {
+#     "joint_state": {
+#         "header": {"seq": 0, "stamp": {"secs": 0, "nsecs": 0}, "frame_id": "base_link"},
+#         "name": [
+#             "shoulder_pan_joint",
+#             "shoulder_lift_joint",
+#             "elbow_joint",
+#             "wrist_1_joint",
+#             "wrist_2_joint",
+#             "wrist_3_joint",
+#         ],
+#         "position": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+#         "velocity": [],
+#         "effort": [],
+#     },
+#     "multi_dof_joint_state": {
+#         "header": {"seq": 0, "stamp": {"secs": 0, "nsecs": 0}, "frame_id": "base_link"},
+#         "joint_names": [],
+#         "transforms": [],
+#         "twist": [],
+#         "wrench": [],
+#     },
+#     "attached_collision_objects": [],
+#     "is_diff": False,
+# }

src/compas_fab/backends/ros/client.py

@@ -30,6 +30,7 @@
 from compas_fab.robots import RobotCell
 from compas_fab.robots import RobotCellState
 
+
 __all__ = [
     "RosClient",
 ]

src/compas_fab/backends/ros/planner.py

@@ -10,6 +10,7 @@
 from compas_fab.backends.interfaces.planner import PlannerInterface
 
 from compas_fab.backends.ros.backend_features import MoveItSetRobotCell
+from compas_fab.backends.ros.backend_features import MoveItSetRobotCellState
 from compas_fab.backends.ros.backend_features import MoveItResetPlanningScene
 from compas_fab.backends.ros.backend_features.move_it_add_attached_collision_mesh import MoveItAddAttachedCollisionMesh
 from compas_fab.backends.ros.backend_features.move_it_add_collision_mesh import MoveItAddCollisionMesh
@@ -23,6 +24,19 @@
     MoveItRemoveAttachedCollisionMesh,
 )
 from compas_fab.backends.ros.backend_features.move_it_remove_collision_mesh import MoveItRemoveCollisionMesh
+from compas_fab.backends.ros.messages import PlanningSceneWorld
+from compas_fab.backends.ros.messages import PlanningScene
+from compas_fab.backends.ros.messages import RobotState
+
+
+from compas import IPY
+
+if not IPY:
+    from typing import TYPE_CHECKING
+
+    if TYPE_CHECKING:
+        from typing import Optional  # noqa: F401
+        from typing import Dict
 
 __all__ = [
     "MoveItPlanner",
@@ -35,13 +49,14 @@ class MoveItPlanner(
     MoveItPlanMotion,
     MoveItPlanCartesianMotion,
     MoveItSetRobotCell,
+    MoveItSetRobotCellState,
     MoveItPlanningScene,
     MoveItResetPlanningScene,
     MoveItAddCollisionMesh,
-    # MoveItRemoveCollisionMesh,
-    # MoveItAppendCollisionMesh,
-    # MoveItAddAttachedCollisionMesh,
-    # MoveItRemoveAttachedCollisionMesh,
+    MoveItRemoveCollisionMesh,
+    MoveItAppendCollisionMesh,
+    MoveItAddAttachedCollisionMesh,
+    MoveItRemoveAttachedCollisionMesh,
     PlannerInterface,
 ):
     """Implement the planner backend interface based on MoveIt!"""
@@ -51,3 +66,26 @@ def __init__(self, client):
 
         # Initialize all mixins
         super(MoveItPlanner, self).__init__()
+
+        self._last_planning_scene_world = None  # type: Optional[PlanningSceneWorld]
+        self._last_robot_state = None  # type: Optional[RobotState]
+
+        self._current_rigid_body_hashes = {}  # type: Dict[str, bytes]
+
+    @property
+    def last_planning_scene_world(self):
+        # type: () -> PlanningSceneWorld
+        if self._last_planning_scene_world is None:
+            planning_scene = self.get_planning_scene()  # type: PlanningScene
+            self._last_planning_scene_world = planning_scene.world
+            self._last_robot_state = planning_scene.robot_state
+        return self._last_planning_scene_world
+
+    @property
+    def last_robot_state(self):
+        # type: () -> RobotState
+        if self._last_robot_state is None:
+            planning_scene = self.get_planning_scene()  # type: PlanningScene
+            self._last_planning_scene_world = planning_scene.world
+            self._last_robot_state = planning_scene.robot_state
+        return self._last_robot_state