diff --git a/examples/ex_collision_mesh.py b/examples/ex_collision_mesh.py index a705652..0fa16fe 100755 --- a/examples/ex_collision_mesh.py +++ b/examples/ex_collision_mesh.py @@ -3,6 +3,7 @@ Example of adding and removing a collision object with a mesh geometry. Note: Python module `trimesh` is required for this example (`pip install trimesh`). - ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.7071, 0.7071]" +- ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="move" -p position:="[0.2, 0.0, 0.2]" - ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p filepath:="./my_favourity_mesh.stl" - ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="remove" """ @@ -80,10 +81,11 @@ def main(): # Determine ID of the collision mesh object_id = path.basename(filepath).split(".")[0] - if "add" == action: + if action == "add": # Add collision mesh node.get_logger().info( - f"Adding collision mesh '{filepath}' {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}" + f"Adding collision mesh '{filepath}' " + f"{{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}" ) moveit2.add_collision_mesh( filepath=filepath, @@ -91,10 +93,21 @@ def main(): position=position, quat_xyzw=quat_xyzw, ) - else: + elif action == "remove": # Remove collision mesh node.get_logger().info(f"Removing collision mesh with ID '{object_id}'") moveit2.remove_collision_object(id=object_id) + elif action == "move": + # Move collision mesh + node.get_logger().info( + f"Moving collision mesh with ID '{object_id}' to " + f"{{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}" + ) + moveit2.move_collision(id=object_id, position=position, quat_xyzw=quat_xyzw) + else: + raise ValueError( + f"Unknown action '{action}'. Valid values are 'add', 'remove', 'move'" + ) rclpy.shutdown() executor_thread.join() diff --git a/examples/ex_collision_primitive.py b/examples/ex_collision_primitive.py index 5d3b2f8..5b2d8d5 100755 --- a/examples/ex_collision_primitive.py +++ b/examples/ex_collision_primitive.py @@ -4,6 +4,7 @@ - ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="sphere" -p position:="[0.5, 0.0, 0.5]" -p dimensions:="[0.04]" - ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="cylinder" -p position:="[0.2, 0.0, -0.045]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p dimensions:="[0.04, 0.02]" - ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p action:="remove" -p shape:="sphere" +- ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p action:="move" -p shape:="sphere" -p position:="[0.2, 0.0, 0.2]" """ from threading import Thread @@ -67,10 +68,11 @@ def main(): # Use the name of the primitive shape as the ID object_id = shape - if "add" == action: + if action == "add": # Add collision primitive node.get_logger().info( - f"Adding collision primitive of type '{shape}' {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}, dimensions: {list(dimensions)}}}" + f"Adding collision primitive of type '{shape}' " + f"{{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}, dimensions: {list(dimensions)}}}" ) if shape == "box": moveit2.add_collision_box( @@ -98,10 +100,21 @@ def main(): ) else: raise ValueError(f"Unknown shape '{shape}'") - else: + elif action == "remove": # Remove collision primitive node.get_logger().info(f"Removing collision primitive with ID '{object_id}'") moveit2.remove_collision_object(id=object_id) + elif action == "move": + # Move collision primitive + node.get_logger().info( + f"Moving collision primitive with ID '{object_id}' to " + f"{{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}" + ) + moveit2.move_collision(id=object_id, position=position, quat_xyzw=quat_xyzw) + else: + raise ValueError( + f"Unknown action '{action}'. Valid values are 'add', 'remove', 'move'" + ) rclpy.shutdown() executor_thread.join() diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index b3918be..90ed568 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1752,6 +1752,44 @@ def detach_all_collision_objects(self): ) self.__attached_collision_object_publisher.publish(msg) + def move_collision( + self, + id: str, + position: Union[Point, Tuple[float, float, float]], + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + frame_id: Optional[str] = None, + ): + """ + Move collision object specified by its `id`. + """ + + msg = CollisionObject() + + if not isinstance(position, Point): + position = Point( + x=float(position[0]), y=float(position[1]), z=float(position[2]) + ) + if not isinstance(quat_xyzw, Quaternion): + quat_xyzw = Quaternion( + x=float(quat_xyzw[0]), + y=float(quat_xyzw[1]), + z=float(quat_xyzw[2]), + w=float(quat_xyzw[3]), + ) + + pose = Pose() + pose.position = position + pose.orientation = quat_xyzw + msg.pose = pose + msg.id = id + msg.operation = CollisionObject.MOVE + msg.header.frame_id = ( + frame_id if frame_id is not None else self.__base_link_name + ) + msg.header.stamp = self._node.get_clock().now().to_msg() + + self.__collision_object_publisher.publish(msg) + def __update_planning_scene(self) -> bool: """ Gets the current planning scene. Returns whether the service call was