Allow clearing all collision objects (#69)

* Enable clearing all collision objects

* Added example

CMakeLists.txt

@@ -12,6 +12,7 @@ ament_python_install_package(pymoveit2)
 set(EXAMPLES_DIR examples)
 install(PROGRAMS
     ${EXAMPLES_DIR}/ex_allow_collisions.py
+    ${EXAMPLES_DIR}/ex_clear_planning_scene.py
     ${EXAMPLES_DIR}/ex_collision_mesh.py
     ${EXAMPLES_DIR}/ex_collision_primitive.py
     ${EXAMPLES_DIR}/ex_fk.py

examples/ex_clear_planning_scene.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+"""
+Example of clearing the planning scene.
+- ros2 run pymoveit2 ex_clear_planning_scene.py"
+- ros2 run pymoveit2 ex_clear_planning_scene.py --ros-args -p cancel_after:=0.0"
+"""
+
+from os import path
+from threading import Thread
+
+import rclpy
+import trimesh
+from rcl_interfaces.msg import ParameterDescriptor, ParameterType
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+
+from pymoveit2 import MoveIt2
+from pymoveit2.robots import panda
+
+
+def main():
+    rclpy.init()
+
+    # Create node for this example
+    node = Node("ex_clear_planning_scene")
+
+    # Declare parameter for joint positions
+    node.declare_parameter(
+        "cancel_after",
+        -1.0,
+        ParameterDescriptor(
+            name="cancel_after",
+            type=ParameterType.PARAMETER_DOUBLE,
+            description=(
+                "The number of seconds after which the service call to clear the "
+                "planning scene should be cancelled. If negative (default), don't cancel."
+            ),
+            read_only=True,
+        ),
+    )
+
+    # Create callback group that allows execution of callbacks in parallel without restrictions
+    callback_group = ReentrantCallbackGroup()
+
+    # Create MoveIt 2 interface
+    moveit2 = MoveIt2(
+        node=node,
+        joint_names=panda.joint_names(),
+        base_link_name=panda.base_link_name(),
+        end_effector_name=panda.end_effector_name(),
+        group_name=panda.MOVE_GROUP_ARM,
+        callback_group=callback_group,
+    )
+
+    # Spin the node in background thread(s) and wait a bit for initialization
+    executor = rclpy.executors.MultiThreadedExecutor(2)
+    executor.add_node(node)
+    executor_thread = Thread(target=executor.spin, daemon=True, args=())
+    executor_thread.start()
+    node.create_rate(1.0).sleep()
+
+    # Get parameters
+    cancel_after = node.get_parameter("cancel_after").get_parameter_value().double_value
+
+    # Clear planning scene
+    future = moveit2.clear_all_collision_objects()
+    start_time = node.get_clock().now()
+    if future is None:
+        node.get_logger().error("Failed to clear planning scene")
+    else:
+        rate = node.create_rate(10)
+        while rclpy.ok() and not future.done():
+            if (
+                cancel_after >= 0.0
+                and (node.get_clock().now() - start_time).nanoseconds / 1e9
+                >= cancel_after
+            ):
+                moveit2.cancel_clear_all_collision_objects_future(future)
+                node.get_logger().info("Cancelled clear planning scene service call")
+                break
+            rate.sleep()
+        if future.cancelled():
+            node.get_logger().info("Cancelled clear planning scene service call")
+        if future.done():
+            success = moveit2.process_clear_all_collision_objects_future(future)
+            if success:
+                node.get_logger().info("Successfully cleared planning scene")
+            else:
+                node.get_logger().error("Failed to clear planning scene")
+
+    rclpy.shutdown()
+    executor_thread.join()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()

pymoveit2/moveit2.py

@@ -240,6 +240,7 @@ def __init__(
             callback_group=callback_group,
         )
         self.__planning_scene = None
+        self.__old_planning_scene = None
         self.__old_allowed_collision_matrix = None
 
         # Create a service for applying the planning scene
@@ -1699,9 +1700,15 @@ def add_collision_mesh(
         # Scale the mesh
         if isinstance(scale, float):
             scale = (scale, scale, scale)
-        transform = np.eye(4)
-        np.fill_diagonal(transform, scale)
-        mesh.apply_transform(transform)
+        if not (scale[0] == scale[1] == scale[2] == 1.0):
+            # If the mesh was passed in as a parameter, make a copy of it to
+            # avoid transforming the original.
+            if filepath is not None:
+                mesh = mesh.copy()
+            # Transform the mesh
+            transform = np.eye(4)
+            np.fill_diagonal(transform, scale)
+            mesh.apply_transform(transform)
 
         msg.meshes.append(
             Mesh(
@@ -1898,6 +1905,54 @@ def process_allow_collision_future(self, future: Future) -> bool:
 
         return resp.success
 
+    def clear_all_collision_objects(self) -> Optional[Future]:
+        """
+        Removes all attached and un-attached collision objects from the planning scene.
+
+        Returns a future for the ApplyPlanningScene service call.
+        """
+        # Update the planning scene
+        if not self.update_planning_scene():
+            return None
+        self.__old_planning_scene = copy.deepcopy(self.__planning_scene)
+
+        # Remove all collision objects from the planning scene
+        self.__planning_scene.world.collision_objects = []
+        self.__planning_scene.robot_state.attached_collision_objects = []
+
+        # Apply the new planning scene
+        if not self._apply_planning_scene_service.service_is_ready():
+            self._node.get_logger().warn(
+                f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
+            )
+            return None
+        return self._apply_planning_scene_service.call_async(
+            ApplyPlanningScene.Request(scene=self.__planning_scene)
+        )
+
+    def cancel_clear_all_collision_objects_future(self, future: Future):
+        """
+        Cancel the clear all collision objects service call.
+        """
+        self._apply_planning_scene_service.remove_pending_request(future)
+
+    def process_clear_all_collision_objects_future(self, future: Future) -> bool:
+        """
+        Return whether the clear all collision objects service call is done and has succeeded
+        or not. If it failed, restore the old planning scene.
+        """
+        if not future.done():
+            return False
+
+        # Get response
+        resp = future.result()
+
+        # If it failed, restore the old planning scene
+        if not resp.success:
+            self.__planning_scene = self.__old_planning_scene
+
+        return resp.success
+
     def __joint_state_callback(self, msg: JointState):
         # Update only if all relevant joints are included in the message
         for joint_name in self.joint_names: