Add ability to move collision objects (#59)

* Added a function to move collision objects

* Add examples

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,21 +81,33 @@ 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,
             id=object_id,
             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()

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()

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