idk how to test this

Author: BergerKing333

src/rovr_control/rovr_control/dig_location_server.py

@@ -1,5 +1,5 @@
 import rclpy
-from rclpy.action import ActionClient, ActionServer
+from rclpy.action import ActionClient, ActionServer, CancelResponse
 from rclpy.node import Node
 from rovr_control.costmap_2d import PyCostmap2D
 from rovr_interfaces.srv import DigLocation
@@ -104,6 +104,14 @@ async def drive_to_dig_location(self, goal_handle: ServerGoalHandle):
 
         return result
 
+    def cancel_callback(self, cancel_request: ServerGoalHandle):
+        self.get_logger().info("Cancelling drive to dig location")
+        if self.nav_handle.status == GoalStatus.STATUS_EXECUTING:
+            self.nav_handle.cancel_goal_async()
+        if not self.nav_handle.is_done():
+            self.nav_handle.cancel_goal_async()
+        return CancelResponse.ACCEPT
+
     # yaw in rads.
     # yaw = 0 at x axis, positive is counter clockwise
     def get_goal_pose(self, x, y, yaw):