diff --git a/src/rovr_control/launch/main_launch.py b/src/rovr_control/launch/main_launch.py index 7e5ca485..f7bf8b71 100644 --- a/src/rovr_control/launch/main_launch.py +++ b/src/rovr_control/launch/main_launch.py @@ -64,6 +64,11 @@ def generate_launch_description(): executable="calibrate_field_coordinate_server", name="calibrate_field_coordinate_server", ) + auto_dig_server = Node( + package="rovr_control", + executable="auto_dig_server", + name="auto_dig_server", + ) auto_offload_server = Node( package="rovr_control", executable="auto_offload_server", @@ -78,6 +83,7 @@ def generate_launch_description(): ld.add_action(read_serial) ld.add_action(can_bus) ld.add_action(calibrate_field_coordinate_server) + ld.add_action(auto_dig_server) ld.add_action(auto_offload_server) return ld diff --git a/src/rovr_control/launch/main_no_joysticks_launch.py b/src/rovr_control/launch/main_no_joysticks_launch.py index 0b4eb738..111ab427 100644 --- a/src/rovr_control/launch/main_no_joysticks_launch.py +++ b/src/rovr_control/launch/main_no_joysticks_launch.py @@ -58,6 +58,11 @@ def generate_launch_description(): executable="calibrate_field_coordinate_server", name="calibrate_field_coordinate_server", ) + auto_dig_server = Node( + package="rovr_control", + executable="auto_dig_server", + name="auto_dig_server", + ) auto_offload_server = Node( package="rovr_control", executable="auto_offload_server", @@ -71,6 +76,7 @@ def generate_launch_description(): ld.add_action(read_serial) ld.add_action(can_bus) ld.add_action(calibrate_field_coordinate_server) + ld.add_action(auto_dig_server) ld.add_action(auto_offload_server) return ld diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py new file mode 100644 index 00000000..58d0d2c7 --- /dev/null +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -0,0 +1,184 @@ +import rclpy +from rclpy.action import ActionServer +from rclpy.node import Node + +from rclpy.action.server import CancelResponse +from std_msgs.msg import Bool + +from rovr_interfaces.action import AutoDig +from rovr_interfaces.srv import Drive, Stop, SetPosition, SetPower + +import time + + +class AutoDigServer(Node): + def __init__(self): + super().__init__("auto_dig_server") + self._action_server = ActionServer( + self, AutoDig, "auto_dig", self.execute_callback, cancel_callback=self.cancel_callback + ) + self.client_node = rclpy.create_node("auto_offload_action_server_client") + + # TODO: This should not be needed anymore after ticket #257 is implemented! + self.skimmer_goal_subscription = self.client_node.create_subscription( + Bool, "/skimmer/goal_reached", self.skimmer_goal_callback, 10 + ) + + self.cli_drivetrain_drive = self.client_node.create_client(Drive, "drivetrain/drive") + self.cli_drivetrain_stop = self.client_node.create_client(Stop, "drivetrain/stop") + + self.cli_lift_zero = self.client_node.create_client(Stop, "lift/zero") + self.cli_lift_setPosition = self.client_node.create_client(SetPosition, "lift/setPosition") + self.cli_lift_set_power = self.client_node.create_client(SetPower, "lift/setPower") + self.cli_lift_stop = self.client_node.create_client(Stop, "lift/stop") + + self.cli_skimmer_stop = self.client_node.create_client(Stop, "skimmer/stop") + self.cli_skimmer_setPower = self.client_node.create_client(SetPower, "skimmer/setPower") + + self.canceled = False + + def execute_callback(self, goal_handle: AutoDig.Goal): + self.get_logger().info("Starting Autonomous Digging Procedure!") + result = AutoDig.Result() + + # Make sure the services are available + if not self.cli_drivetrain_drive.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Drivetrain drive service not available") + goal_handle.abort() + return result + if not self.cli_drivetrain_stop.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Drivetrain stop service not available") + goal_handle.abort() + return result + if not self.cli_lift_setPosition.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Lift set position service not available") + goal_handle.abort() + return result + if not self.cli_lift_stop.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Lift stop service not available") + goal_handle.abort() + return result + if not self.cli_lift_set_power.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Lift set power service not available") + goal_handle.abort() + return result + if not self.cli_lift_zero.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Lift zero service not available") + goal_handle.abort() + return result + if not self.cli_skimmer_setPower.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Skimmer set power service not available") + goal_handle.abort() + return result + if not self.cli_skimmer_stop.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Skimmer stop service not available") + goal_handle.abort() + return result + + # Zero the skimmer + self.cli_lift_zero.call_async(Stop.Request()) + # Wait for the lift goal to be reached + self.skimmer_goal_reached = False + while not self.skimmer_goal_reached: + self.get_logger().info("Moving the lift to the goal") + rclpy.spin_once(self.client_node, timeout_sec=0) # Allows for task to be canceled + if self.canceled: + self.get_logger().warn("Zeroing the Lift was Canceled!\n") + self.canceled = False + goal_handle.canceled() + return result + time.sleep(0.1) + + # Lower the skimmer onto the ground + self.cli_lift_setPosition.call_async( + SetPosition.Request(position=goal_handle.request.lift_digging_start_position) + ) + # Wait for the lift goal to be reached + self.skimmer_goal_reached = False + while not self.skimmer_goal_reached: + self.get_logger().info("Moving the lift to the goal") + rclpy.spin_once(self.client_node, timeout_sec=0) # Allows for task to be canceled + if self.canceled: + self.get_logger().warn("Lowering the Lift was Canceled!\n") + self.canceled = False + goal_handle.canceled() + return result + time.sleep(0.1) + + # Start the skimmer belt + self.cli_skimmer_setPower.call_async(SetPower.Request(power=goal_handle.request.skimmer_belt_power)) + + # Drive forward while digging + start_time = self.get_clock().now().nanoseconds + elapsed = self.get_clock().now().nanoseconds - start_time + # accelerate for 2 seconds + # TODO make the ramp up a service so this while loop isn't needed + while elapsed < 2e9: + self.cli_lift_set_power.call_async(SetPower.Request(power=-0.05e-9 * (elapsed))) + self.cli_drivetrain_drive.call_async( + Drive.Request(forward_power=0.0, horizontal_power=0.25e-9 * (elapsed), turning_power=0.0) + ) + self.get_logger().info("Accelerating lift and drive train") + elapsed = self.get_clock().now().nanoseconds - start_time + rclpy.spin_once(self.client_node, timeout_sec=0) # Allows for task to be canceled + if self.canceled: + self.get_logger().warn("Digging Acceleration Canceled!\n") + self.canceled = False + goal_handle.canceled() + return result + time.sleep(0.1) + + self.get_logger().info("Auto Driving") + # keep digging at full speed for the remaining 10 seconds + while self.get_clock().now().nanoseconds - start_time < 12e9: + rclpy.spin_once(self.client_node, timeout_sec=0) # Allows for task to be canceled + if self.canceled: + self.get_logger().warn("Digging Canceled!\n") + self.canceled = False + goal_handle.canceled() + return result + time.sleep(0.1) + + # Stop driving and skimming + self.cli_drivetrain_stop.call_async(Stop.Request()) + self.cli_skimmer_stop.call_async(Stop.Request()) + + self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) + # Wait for the lift goal to be reached + self.skimmer_goal_reached = False + while not self.skimmer_goal_reached: + self.get_logger().info("Moving the lift to the goal") + rclpy.spin_once(self.client_node, timeout_sec=0) # Allows for task to be canceled + if self.canceled: + self.get_logger().warn("Raising the Lift was Canceled!\n") + self.canceled = False + goal_handle.canceled() + return result + time.sleep(0.1) + + self.get_logger().info("Autonomous Digging Procedure Complete!\n") + goal_handle.succeed() + return result + + def cancel_callback(self, cancel_request): + """This method is called when the action is canceled.""" + self.get_logger().info("Goal was canceled") + self.canceled = True + return CancelResponse.ACCEPT + + # TODO: This should not be needed anymore after ticket #257 is implemented! + def skimmer_goal_callback(self, msg: Bool) -> None: + """Update the member variable accordingly.""" + self.skimmer_goal_reached = msg.data + + +def main(args=None) -> None: + rclpy.init(args=args) + action_server = AutoDigServer() + rclpy.spin(action_server) + rclpy.shutdown() + + +# This code does NOT run if this file is imported as a module +if __name__ == "__main__": + main() diff --git a/src/rovr_control/rovr_control/auto_offload_server.py b/src/rovr_control/rovr_control/auto_offload_server.py index e90e557e..00fd3230 100644 --- a/src/rovr_control/rovr_control/auto_offload_server.py +++ b/src/rovr_control/rovr_control/auto_offload_server.py @@ -18,6 +18,8 @@ def __init__(self): self, AutoOffload, "auto_offload", self.execute_callback, cancel_callback=self.cancel_callback ) self.client_node = rclpy.create_node("auto_offload_action_server_client") + + # TODO: This should not be needed anymore after ticket #257 is implemented! self.skimmer_goal_subscription = self.client_node.create_subscription( Bool, "/skimmer/goal_reached", self.skimmer_goal_callback, 10 ) @@ -35,7 +37,7 @@ def __init__(self): def execute_callback(self, goal_handle: AutoOffload.Goal): """This method lays out the procedure for autonomously offloading!""" - self.get_logger().info("Executing goal...") + self.get_logger().info("Starting Autonomous Offload Procedure!") result = AutoOffload.Result() # Make sure the services are available @@ -84,10 +86,10 @@ def execute_callback(self, goal_handle: AutoOffload.Goal): # Raise up the skimmer in preparation for dumping self.get_logger().info("Raising the Lift!") self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) - self.skimmer_goal_reached = False # Wait for the lift goal to be reached + self.skimmer_goal_reached = False while not self.skimmer_goal_reached: - self.get_logger().info("Moving skimmer to the goal") + self.get_logger().info("Moving the lift to the goal") rclpy.spin_once(self.client_node, timeout_sec=0) # Allows for task to be canceled if self.canceled: self.get_logger().warn("Raising the Lift was Canceled!\n") diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 3f7cb1dd..12048583 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -25,7 +25,7 @@ # Import custom ROS 2 interfaces from rovr_interfaces.srv import Stop, Drive, MotorCommandGet, SetPower, SetPosition -from rovr_interfaces.action import CalibrateFieldCoordinates, AutoOffload +from rovr_interfaces.action import CalibrateFieldCoordinates, AutoDig, AutoOffload # Import Python Modules import asyncio # Allows the use of asynchronous methods! @@ -103,13 +103,6 @@ def __init__(self) -> None: # Define some initial states here self.state = states["Teleop"] - self.camera_view_toggled = False - self.front_camera = None - self.back_camera = None - self.autonomous_digging_process = None - self.autonomous_offload_process = None - self.autonomous_cycle_process = None - self.skimmer_goal_reached = True self.DANGER_THRESHOLD = 1 self.REAL_DANGER_THRESHOLD = 100 @@ -150,10 +143,13 @@ def __init__(self) -> None: self.act_calibrate_field_coordinates = ActionClient( self, CalibrateFieldCoordinates, "calibrate_field_coordinates" ) + self.act_auto_dig = ActionClient(self, AutoDig, "auto_dig") self.act_auto_offload = ActionClient(self, AutoOffload, "auto_offload") self.field_calibrated_handle: ClientGoalHandle = ClientGoalHandle(None, None, None) + self.auto_dig_handle: ClientGoalHandle = ClientGoalHandle(None, None, None) self.auto_offload_handle: ClientGoalHandle = ClientGoalHandle(None, None, None) + self.nav2 = BasicNavigator() # Instantiate the BasicNavigator class # ----- !! BLOCKING WHILE LOOP !! ----- # @@ -206,58 +202,6 @@ def end_autonomous(self) -> None: self.stop_all_subsystems() # Stop all subsystems self.state = states["Teleop"] # Return to Teleop mode - # TODO: This autonomous routine has not been tested yet! - async def auto_dig_procedure(self) -> None: - """This method lays out the procedure for autonomously digging!""" - self.get_logger().info("Starting Autonomous Digging Procedure!") - try: # Wrap the autonomous procedure in a try-except - await self.cli_lift_zero.call_async(Stop.Request()) - await self.cli_lift_setPosition.call_async( - SetPosition.Request(position=self.lift_digging_start_position) - ) # Lower the skimmer onto the ground - self.skimmer_goal_reached = False - # Wait for the goal height to be reached - while not self.skimmer_goal_reached: - self.get_logger().info("Moving skimmer to starting dig position") - await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) - await self.cli_skimmer_setPower.call_async(SetPower.Request(power=self.skimmer_belt_power)) - # Drive forward while digging - start_time = self.get_clock().now().nanoseconds - elapsed = self.get_clock().now().nanoseconds - start_time - # accelerate for 2 seconds - while elapsed < 2e9: - await self.cli_lift_set_power.call_async(SetPower.Request(power=-0.05e-9 * (elapsed))) - await self.cli_drivetrain_drive.call_async( - Drive.Request(forward_power=0.0, horizontal_power=0.25e-9 * (elapsed), turning_power=0.0) - ) - self.get_logger().info("Accelerating lift and drive train") - elapsed = self.get_clock().now().nanoseconds - start_time - await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) - # keep digging at full speed for the remaining 10 seconds - while self.get_clock().now().nanoseconds - start_time < 12e9: - self.get_logger().info("Auto Driving") - await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) - await self.cli_drivetrain_stop.call_async(Stop.Request()) - await self.cli_skimmer_stop.call_async(Stop.Request()) - await self.cli_lift_setPosition.call_async( - SetPosition.Request(position=self.lift_dumping_position) - ) # Raise the skimmer back up - self.skimmer_goal_reached = False - # Wait for the lift goal to be reached - while not self.skimmer_goal_reached: - self.get_logger().info("Moving skimmer to dumping position") - await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) - self.get_logger().info("Autonomous Digging Procedure Complete!") - self.end_autonomous() # Return to Teleop mode - except asyncio.CancelledError: # Put termination code here - self.get_logger().warn("Autonomous Digging Procedure Terminated") - self.end_autonomous() # Return to Teleop mode - - # TODO: This should not be needed anymore after ticket #257 is implemented! - def skimmer_goal_callback(self, msg: Bool) -> None: - """Update the member variable accordingly.""" - self.skimmer_goal_reached = msg.data - def get_result_callback(self, future: Future): goal_handle: ClientGoalHandle = future.result() if goal_handle.status == GoalStatus.STATUS_SUCCEEDED: @@ -280,6 +224,14 @@ def auto_offload_goal_response_callback(self, future: Future): result: Future = self.auto_offload_handle.get_result_async() result.add_done_callback(self.get_result_callback) + def auto_dig_goal_response_callback(self, future: Future): + self.auto_dig_handle: ClientGoalHandle = future.result() + if not self.auto_dig_handle.accepted: + self.get_logger().info("Auto dig Goal rejected") + return + result: Future = self.auto_dig_handle.get_result_async() + result.add_done_callback(self.get_result_callback) + def joystick_callback(self, msg: Joy) -> None: """This method is called whenever a joystick message is received.""" @@ -293,7 +245,10 @@ def joystick_callback(self, msg: Joy) -> None: ) # Horizontal power turn_power = msg.axes[bindings.LEFT_JOYSTICK_HORIZONTAL_AXIS] * self.max_turn_power # Turning power self.drive_power_publisher.publish( - Twist(linear=Vector3(x=forward_power, y=horizontal_power), angular=Vector3(z=turn_power)) + Twist( + linear=Vector3(x=forward_power, y=horizontal_power), + angular=Vector3(z=turn_power), + ) ) # Check if the skimmer button is pressed # @@ -350,15 +305,25 @@ def joystick_callback(self, msg: Joy) -> None: # Check if the autonomous digging button is pressed # TODO: This needs to be tested extensively on the physical robot! if msg.buttons[bindings.BACK_BUTTON] == 1 and buttons[bindings.BACK_BUTTON] == 0: - if self.state == states["Teleop"]: + # Check if the auto digging process is not running + if self.auto_dig_handle.status != GoalStatus.STATUS_EXECUTING: + if not self.act_auto_dig.wait_for_server(timeout_sec=1.0): + self.get_logger().error("Auto dig action not available") + return + goal = AutoDig.Goal( + lift_dumping_position=self.lift_dumping_position, + lift_digging_start_position=self.lift_digging_start_position, + skimmer_belt_power=self.skimmer_belt_power, + ) + auto_dig_request = self.act_auto_dig.send_goal_async(goal) + auto_dig_request.add_done_callback(self.auto_dig_goal_response_callback) self.stop_all_subsystems() # Stop all subsystems self.state = states["Autonomous"] # Exit Teleop mode - self.autonomous_digging_process = asyncio.ensure_future( - self.auto_dig_procedure() - ) # Start the auto dig process - elif self.state == states["Autonomous"]: - self.autonomous_digging_process.cancel() # Terminate the auto dig process - self.autonomous_digging_process = None + # Terminate the auto dig process + else: + self.get_logger().warn("Auto Dig Terminated") + self.auto_dig_handle.cancel_goal_async() + self.end_autonomous() # Return to Teleop mode # Check if the autonomous offload button is pressed # TODO: This needs to be tested extensively on the physical robot! diff --git a/src/rovr_control/setup.py b/src/rovr_control/setup.py index 83734f36..7cd5ba40 100644 --- a/src/rovr_control/setup.py +++ b/src/rovr_control/setup.py @@ -29,6 +29,7 @@ "main_control_node = rovr_control.main_control_node:main", "read_serial = rovr_control.read_serial:main", "calibrate_field_coordinate_server = rovr_control.calibrate_field_coordinate_server:main", + "auto_dig_server = rovr_control.auto_dig_server:main", "auto_offload_server = rovr_control.auto_offload_server:main", } ], diff --git a/src/rovr_interfaces/CMakeLists.txt b/src/rovr_interfaces/CMakeLists.txt index 4c9963c6..bf38e456 100644 --- a/src/rovr_interfaces/CMakeLists.txt +++ b/src/rovr_interfaces/CMakeLists.txt @@ -19,6 +19,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "action/AutoDig.action" "action/AutoOffload.action" "action/CalibrateFieldCoordinates.action" "msg/AbsoluteEncoders.msg" diff --git a/src/rovr_interfaces/action/AutoDig.action b/src/rovr_interfaces/action/AutoDig.action new file mode 100644 index 00000000..96053ba9 --- /dev/null +++ b/src/rovr_interfaces/action/AutoDig.action @@ -0,0 +1,8 @@ +# Request +float64 lift_dumping_position +float64 lift_digging_start_position +float64 skimmer_belt_power +--- +# Result +--- +# Feedback \ No newline at end of file diff --git a/src/skimmer/skimmer/skimmer_node.py b/src/skimmer/skimmer/skimmer_node.py index b570992b..82253acb 100644 --- a/src/skimmer/skimmer/skimmer_node.py +++ b/src/skimmer/skimmer/skimmer_node.py @@ -101,6 +101,7 @@ def toggle(self, skimmer_belt_power: float) -> None: else: self.set_power(skimmer_belt_power) + # TODO: This method can probably be deleted during the implementation of ticket #257 def set_position(self, position: int) -> None: """This method sets the position (in degrees) of the skimmer.""" self.current_goal_position = position # goal position should be in degrees @@ -157,6 +158,9 @@ def toggle_callback(self, request, response): response.success = 0 # indicates success return response + # TODO: This method needs to be modified during the implementation of ticket #257 + # to return a proper future indicating when the goal position has been reached + # so that rclpy.spin_until_future_complete() can be used to wait for the goal. def set_position_callback(self, request, response): """This service request sets the position of the lift.""" self.set_position(request.position)