From 58a9d372463159af8bf4f02b1e9300b904596a89 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Mon, 13 May 2024 17:57:15 -0700 Subject: [PATCH 1/2] Enable clearing all collision objects --- examples/ex_clear_planning_scene.py | 0 pymoveit2/moveit2.py | 61 +++++++++++++++++++++++++++-- 2 files changed, 58 insertions(+), 3 deletions(-) create mode 100644 examples/ex_clear_planning_scene.py diff --git a/examples/ex_clear_planning_scene.py b/examples/ex_clear_planning_scene.py new file mode 100644 index 0000000..e69de29 diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 1a5619b..4aca121 100644 --- a/pymoveit2/moveit2.py +++ b/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: From 63ef49546f49d2fa99fa6a197d7a9e092dad6051 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 15 May 2024 09:30:33 -0700 Subject: [PATCH 2/2] Added example --- CMakeLists.txt | 1 + examples/ex_clear_planning_scene.py | 97 +++++++++++++++++++++++++++++ 2 files changed, 98 insertions(+) mode change 100644 => 100755 examples/ex_clear_planning_scene.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e195a8..910af21 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/examples/ex_clear_planning_scene.py b/examples/ex_clear_planning_scene.py old mode 100644 new mode 100755 index e69de29..ad6c83e --- a/examples/ex_clear_planning_scene.py +++ b/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()