Re-Added Async_sleep via asyncnode to control time spent digging and …

…dumping via yaml config dig_time and dump_time, set to 5 seconds atm in auto dig and dumper_node - we should probably standardize this

config/rovr_control.yaml

@@ -9,3 +9,5 @@
     lift_dumping_position: -1000 # Measured in encoder counts!
     lift_digging_start_position: -3050 # Measured in encoder counts!
     lift_digging_end_position: -3150 # Measured in encoder counts!
+    dig_time: 5
+    dump_time: 5

src/dumper/dumper/dumper_node.py

@@ -35,9 +35,11 @@ def __init__(self):
         # Define default values for our ROS parameters below #
         self.declare_parameter("DUMPER_MOTOR", 11)
         self.declare_parameter("DUMPER_POWER", 0.5)
+        self.declare_parameter("dump_time", 5)
         # Assign the ROS Parameters to member variables below #
         self.DUMPER_MOTOR = self.get_parameter("DUMPER_MOTOR").value
         self.DUMPER_POWER = self.get_parameter("DUMPER_POWER").value
+        self.dumpTime = self.get_parameter("dump_time").value
 
         # Print the ROS Parameters to the terminal below #
         self.get_logger().info("DUMPER_MOTOR has been set to: " + str(self.DUMPER_MOTOR))
@@ -122,7 +124,7 @@ def dump(self) -> None:
         # extend the dumper
         self.extend_dumper()
         # wait for 5 seconds before retracting the dumper
-        time.sleep(5)
+        time.sleep(self.dumpTime)
         # retract the dumper
         self.retract_dumper()
 

src/rovr_control/launch/main_launch.py

@@ -50,7 +50,7 @@ def generate_launch_description():
         package="dumper",
         executable="dumper_node",
         name="dumper_node",
-        parameters=["config/dumper_config.yaml", "config/motor_control.yaml"],
+        parameters=["config/dumper_config.yaml", "config/motor_control.yaml","config/rovr_control.yaml"],
         output="screen",
     )
 

src/rovr_control/launch/main_no_joysticks_launch.py

@@ -44,7 +44,7 @@ def generate_launch_description():
         package="dumper",
         executable="dumper_node",
         name="dumper_node",
-        parameters=["config/motor_control.yaml"],
+        parameters=["config/motor_control.yaml","config/rovr_control.yaml"],
         output="screen",
     )
 

src/rovr_control/rovr_control/auto_dig_server.py

@@ -1,16 +1,15 @@
 import rclpy
 from rclpy.node import Node
+from rovr_control.node_util import AsyncNode
 from rclpy.action import ActionServer
 
 from rovr_interfaces.action import AutoDig
 from rovr_interfaces.srv import Drive, SetPosition, SetPower
 from rclpy.action.server import ServerGoalHandle, CancelResponse
 from std_srvs.srv import Trigger
 
-from asyncio import sleep
 
-
-class AutoDigServer(Node):
+class AutoDigServer(AsyncNode):
     def __init__(self):
         super().__init__("auto_dig_server")
         self._action_server = ActionServer(
@@ -29,6 +28,9 @@ def __init__(self):
         self.cli_digger_stop = self.create_client(Trigger, "digger/stop")
         self.cli_digger_setPower = self.create_client(SetPower, "digger/setPower")
 
+        self.declare_parameter("dig_time", 5)
+        self.digTime = self.get_parameter("dig_time").value
+
     async def execute_callback(self, goal_handle: ServerGoalHandle):
         self.get_logger().info("Starting Autonomous Digging Procedure!")
         result = AutoDig.Result()
@@ -77,7 +79,7 @@ async def execute_callback(self, goal_handle: ServerGoalHandle):
         await self.cli_lift_lower.call_async(Trigger.Reqest())
 
         self.get_logger().info("Start of Auto Digging in Place")
-        await sleep(3)  # Stay at lowest pos for 3 seconds while digging
+        await self.async_sleep(self.digTime)  # Stay at lowest pos for 3 seconds while digging
         self.get_logger().info("Done Digging in Place")
 
         # Stop skimming

src/rovr_control/rovr_control/node_util.py

@@ -0,0 +1,33 @@
+from rclpy.action.server import CancelResponse, ServerGoalHandle
+from rclpy.node import Node
+from rclpy.task import Future
+
+from std_msgs.msg import Bool
+
+
+class AsyncNode(Node):
+    def __init__(self, name: str):
+        super().__init__(name)
+        self.sleep_goal_reached = Future()
+        self.sleep_goal_reached.set_result(None)
+
+        self.timer = None
+
+    # Temporary measure to sleep for time without needlessly spinning
+    async def async_sleep(self, seconds: float) -> None:
+        self.sleep_goal_reached = Future()
+
+        def handler():
+            self.sleep_goal_reached.set_result(None)
+
+        self.timer = self.create_timer(seconds, handler)
+        await self.sleep_goal_reached
+        self.timer.cancel()
+        self.timer.destroy()
+
+    def cancel_callback(self, cancel_request: ServerGoalHandle) -> None:
+        if not self.sleep_goal_reached.done():
+            self.timer.cancel()
+            self.timer.destroy()
+            return CancelResponse.ACCEPT
+        return CancelResponse.REJECT