From 5c98c98aa22cb281a5e0fc6c07da81353717a5f2 Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Sat, 21 Sep 2024 10:43:12 -0500 Subject: [PATCH 01/45] Initial Commit of the Drivetrain Refactor --- config/drivetrain_config.yaml | 10 ++-------- config/motor_control.yaml | 4 ---- src/rovr_control/rovr_control/main_control_node.py | 14 +++----------- 3 files changed, 5 insertions(+), 23 deletions(-) diff --git a/config/drivetrain_config.yaml b/config/drivetrain_config.yaml index 271d3d3e..69057b45 100644 --- a/config/drivetrain_config.yaml +++ b/config/drivetrain_config.yaml @@ -1,11 +1,5 @@ /**: ros__parameters: - HALF_WHEEL_BASE: 0.55959375 # HALF of the wheelbase in meters - HALF_TRACK_WIDTH: 0.2746375 # HALF of the trackwidth in meters - STEERING_MOTOR_GEAR_RATIO: 40 # Gear ratio of the steering motor (40:1) - FRONT_LEFT_MAGNET_OFFSET: 97 # (in encoder counts) - FRONT_RIGHT_MAGNET_OFFSET: 873 # (in encoder counts) - BACK_LEFT_MAGNET_OFFSET: 50 # (in encoder counts) - BACK_RIGHT_MAGNET_OFFSET: 647 # (in encoder counts) - ABSOLUTE_ENCODER_COUNTS: 1024 # (in encoder counts) + HALF_WHEEL_BASE: 0.55959375 # HALF of the wheelbase in meters # TODO: Update this for the 2024-2025 robot + HALF_TRACK_WIDTH: 0.2746375 # HALF of the trackwidth in meters # TODO: Update this for the 2024-2025 robot GAZEBO_SIMULATION: False # Set to true if running in Gazebo diff --git a/config/motor_control.yaml b/config/motor_control.yaml index 6dfa3532..608c36aa 100644 --- a/config/motor_control.yaml +++ b/config/motor_control.yaml @@ -3,12 +3,8 @@ CAN_INTERFACE_TRANSMIT: "can0" # can0, can1, slcan0, slcan1 are common values CAN_INTERFACE_RECEIVE: "can0" # can0, can1, slcan0, slcan1 are common values BACK_LEFT_DRIVE: 7 # CAN ID of the back left drive motor - BACK_LEFT_TURN: 6 # CAN ID of the back left turn motor FRONT_LEFT_DRIVE: 10 # CAN ID of the front left drive motor - FRONT_LEFT_TURN: 4 # CAN ID of the front left turn motor BACK_RIGHT_DRIVE: 8 # CAN ID of the back right drive motor - BACK_RIGHT_TURN: 5 # CAN ID of the back right turn motor FRONT_RIGHT_DRIVE: 9 # CAN ID of the front right drive motor - FRONT_RIGHT_TURN: 3 # CAN ID of the front right turn motor SKIMMER_BELT_MOTOR: 2 # CAN ID of the skimmer belt motor SKIMMER_LIFT_MOTOR: 1 # CAN ID of the skimmer lift motor diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index f84bcaf8..24d80275 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -217,18 +217,10 @@ async def joystick_callback(self, msg: Joy) -> None: # PUT TELEOP CONTROLS BELOW # if self.state == states["Teleop"]: - # Drive the robot using joystick input during Teleop + # Drive the robot using joystick input during Teleop (Arcade Drive) forward_power = msg.axes[bindings.RIGHT_JOYSTICK_VERTICAL_AXIS] * self.max_drive_power # Forward power - horizontal_power = ( - msg.axes[bindings.RIGHT_JOYSTICK_HORIZONTAL_AXIS] * self.max_drive_power - ) # 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), - ) - ) + turning_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), angular=Vector3(z=turning_power))) # Check if the skimmer button is pressed # if msg.buttons[bindings.X_BUTTON] == 1 and buttons[bindings.X_BUTTON] == 0: From a976131cfda1f7af34edc869cd66d4a9695a9c06 Mon Sep 17 00:00:00 2001 From: kznhq <145292978+kznhq@users.noreply.github.com> Date: Tue, 24 Sep 2024 23:56:38 +0000 Subject: [PATCH 02/45] refactored read_serial.py to not use absolute encoders per task #275 "Refactor swerve drive into tank drive" --- src/rovr_control/rovr_control/read_serial.py | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/src/rovr_control/rovr_control/read_serial.py b/src/rovr_control/rovr_control/read_serial.py index 4aae5f39..adb9d5db 100644 --- a/src/rovr_control/rovr_control/read_serial.py +++ b/src/rovr_control/rovr_control/read_serial.py @@ -1,6 +1,6 @@ import rclpy from rclpy.node import Node -from rovr_interfaces.msg import LimitSwitches, AbsoluteEncoders +from rovr_interfaces.msg import LimitSwitches import serial import struct @@ -12,7 +12,6 @@ def __init__(self): super().__init__("read_serial") self.limitSwitchesPub = self.create_publisher(LimitSwitches, "limitSwitches", 10) - self.absoluteEncodersPub = self.create_publisher(AbsoluteEncoders, "absoluteEncoders", 10) try: self.arduino = serial.Serial("/dev/ttyACM0", 9600) @@ -29,21 +28,13 @@ def __init__(self): self.destroy_node() return data = self.arduino.read(10) # Pause until 10 bytes are read - decoded = struct.unpack("??hhhh", data) # Use h for each int because arduino int is 2 bytes + decoded = struct.unpack("??", data) # Use h for each int because arduino int is 2 bytes msg = LimitSwitches() msg.top_limit_switch = decoded[0] msg.bottom_limit_switch = decoded[1] self.limitSwitchesPub.publish(msg) - # NOTE: swerve module 90 degree switch, the old version was decoded[2-5] in order - msg = AbsoluteEncoders() - msg.front_left_encoder = decoded[4] - msg.front_right_encoder = decoded[2] - msg.back_left_encoder = decoded[5] - msg.back_right_encoder = decoded[3] - self.absoluteEncodersPub.publish(msg) - def main(args=None): """The main function.""" From 3ef8cc754d2778ec6120d72c3e3d79a0760b48da Mon Sep 17 00:00:00 2001 From: andrewtomfohrde Date: Wed, 25 Sep 2024 00:04:05 +0000 Subject: [PATCH 03/45] Removed instantiated PID controllers --- src/motor_control/src/motor_control_node.cpp | 27 +++----------------- 1 file changed, 3 insertions(+), 24 deletions(-) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index 5966cc0c..c92fad30 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -227,24 +227,14 @@ class MotorControlNode : public rclcpp::Node { public: MotorControlNode() : Node("MotorControlNode") { // Define default values for our ROS parameters below # - this->declare_parameter("CAN_INTERFACE_TRANSMIT", "can0"); + this->declare_parameter("CAN_INTERFACE_TRANSMIT", "can0"); this->declare_parameter("CAN_INTERFACE_RECEIVE", "can0"); - this->declare_parameter("BACK_LEFT_TURN", 4); - this->declare_parameter("FRONT_LEFT_TURN", 3); - this->declare_parameter("BACK_RIGHT_TURN", 6); - this->declare_parameter("FRONT_RIGHT_TURN", 5); this->declare_parameter("SKIMMER_LIFT_MOTOR", 1); - this->declare_parameter("STEERING_MOTOR_GEAR_RATIO", 40); // Print the ROS Parameters to the terminal below # - RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_TRANSMIT parameter set to: %s", this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string().c_str()); + RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_TRANSMIT parameter set to: %s", this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string().c_str()); RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_RECEIVE parameter set to: %s", this->get_parameter("CAN_INTERFACE_RECEIVE").as_string().c_str()); - RCLCPP_INFO(this->get_logger(), "BACK_LEFT_TURN parameter set to: %ld", this->get_parameter("BACK_LEFT_TURN").as_int()); - RCLCPP_INFO(this->get_logger(), "FRONT_LEFT_TURN parameter set to: %ld", this->get_parameter("FRONT_LEFT_TURN").as_int()); - RCLCPP_INFO(this->get_logger(), "BACK_RIGHT_TURN parameter set to: %ld", this->get_parameter("BACK_RIGHT_TURN").as_int()); - RCLCPP_INFO(this->get_logger(), "FRONT_RIGHT_TURN parameter set to: %ld", this->get_parameter("FRONT_RIGHT_TURN").as_int()); RCLCPP_INFO(this->get_logger(), "SKIMMER_LIFT_MOTOR parameter set to: %ld", this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()); - RCLCPP_INFO(this->get_logger(), "STEERING_MOTOR_GEAR_RATIO parameter set to: %ld", this->get_parameter("STEERING_MOTOR_GEAR_RATIO").as_int()); // Initialize services below // srv_motor_set = this->create_service( @@ -252,19 +242,8 @@ class MotorControlNode : public rclcpp::Node { srv_motor_get = this->create_service( "motor/get", std::bind(&MotorControlNode::get_callback, this, _1, _2)); - // Instantiate all of our PIDControllers here - this->pid_controllers[this->get_parameter("BACK_LEFT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.4); - this->pid_controllers[this->get_parameter("FRONT_LEFT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.4); - this->pid_controllers[this->get_parameter("BACK_RIGHT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.4); - this->pid_controllers[this->get_parameter("FRONT_RIGHT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.4); this->pid_controllers[this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 20, 0.05); - - // Enable continuous input for the swerve module PID controllers - this->pid_controllers[this->get_parameter("BACK_LEFT_TURN").as_int()]->enableContinuousInput(0, 360 * this->get_parameter("STEERING_MOTOR_GEAR_RATIO").as_int()); - this->pid_controllers[this->get_parameter("FRONT_LEFT_TURN").as_int()]->enableContinuousInput(0, 360 * this->get_parameter("STEERING_MOTOR_GEAR_RATIO").as_int()); - this->pid_controllers[this->get_parameter("BACK_RIGHT_TURN").as_int()]->enableContinuousInput(0, 360 * this->get_parameter("STEERING_MOTOR_GEAR_RATIO").as_int()); - this->pid_controllers[this->get_parameter("FRONT_RIGHT_TURN").as_int()]->enableContinuousInput(0, 360 * this->get_parameter("STEERING_MOTOR_GEAR_RATIO").as_int()); - + // Initialize timers below // timer = this->create_wall_timer(500ms, std::bind(&MotorControlNode::timer_callback, this)); From 9e649142f2e3d145a77b52eafa755b02a556770d Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Tue, 24 Sep 2024 19:07:36 -0500 Subject: [PATCH 04/45] deleted all absolute encoders from the arduino code --- arduino/sensors/sensors.ino | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/arduino/sensors/sensors.ino b/arduino/sensors/sensors.ino index 377cb7ed..f9543753 100644 --- a/arduino/sensors/sensors.ino +++ b/arduino/sensors/sensors.ino @@ -2,17 +2,9 @@ struct SensorData { bool topLimitSwitch; bool bottomLimitSwitch; - uint16_t frontLeftEncoder; - uint16_t frontRightEncoder; - uint16_t backLeftEncoder; - uint16_t backRightEncoder; }; // Define the sensor pins here -#define FRONT_LEFT_ENCODER A0 -#define FRONT_RIGHT_ENCODER A1 -#define BACK_LEFT_ENCODER A2 -#define BACK_RIGHT_ENCODER A3 #define TOP_LIMIT_SWITCH 2 #define BOTTOM_LIMIT_SWITCH 3 @@ -20,12 +12,6 @@ void setup() { // Initialize serial communication Serial.begin(9600); - // Initialize the analog inputs (absolute encoders) - pinMode(FRONT_LEFT_ENCODER, INPUT); - pinMode(FRONT_RIGHT_ENCODER, INPUT); - pinMode(BACK_LEFT_ENCODER, INPUT); - pinMode(BACK_RIGHT_ENCODER, INPUT); - // Initialize the digital inputs (limit switches) pinMode(TOP_LIMIT_SWITCH, INPUT_PULLUP); pinMode(BOTTOM_LIMIT_SWITCH, INPUT_PULLUP); @@ -35,12 +21,6 @@ void loop() { // Create a SensorData struct SensorData data; - // Read from the analog inputs (absolute encoders) - data.frontLeftEncoder = (uint16_t)analogRead(FRONT_LEFT_ENCODER); - data.frontRightEncoder = (uint16_t)analogRead(FRONT_RIGHT_ENCODER); - data.backLeftEncoder = (uint16_t)analogRead(BACK_LEFT_ENCODER); - data.backRightEncoder = (uint16_t)analogRead(BACK_RIGHT_ENCODER); - // Read from the digital inputs (limit switches) data.topLimitSwitch = (bool)digitalRead(TOP_LIMIT_SWITCH); data.bottomLimitSwitch = (bool)digitalRead(BOTTOM_LIMIT_SWITCH); From 0c1b0b8e01a37d5a3151f7f669796ff65c53ccff Mon Sep 17 00:00:00 2001 From: Ashton Date: Wed, 25 Sep 2024 00:33:00 +0000 Subject: [PATCH 05/45] refactored drivetrain_node to arcade drive from swerve Co-authored-by: cparece1 --- src/drivetrain/drivetrain/drivetrain_node.py | 316 ++----------------- 1 file changed, 31 insertions(+), 285 deletions(-) diff --git a/src/drivetrain/drivetrain/drivetrain_node.py b/src/drivetrain/drivetrain/drivetrain_node.py index 670fc4ba..74874cee 100644 --- a/src/drivetrain/drivetrain/drivetrain_node.py +++ b/src/drivetrain/drivetrain/drivetrain_node.py @@ -1,7 +1,7 @@ -# This ROS 2 node contains code for the swerve drivetrain subsystem of the robot. +# This ROS 2 node contains code for the drivetrain subsystem of the robot. # Original Author: Akshat Arinav in Fall 2023 # Maintainer: Anthony Brogni -# Last Updated: February 2024 by Anthony Brogni +# Last Updated: September 2024 by Charlie & Ashton import math @@ -15,66 +15,7 @@ # Import custom ROS 2 interfaces from rovr_interfaces.srv import Stop, Drive, MotorCommandSet, MotorCommandGet -from rovr_interfaces.msg import AbsoluteEncoders - -# This class represents an individual swerve module -class SwerveModule: - def __init__(self, drive_motor, turning_motor, drivetrain): - self.drive_motor_can_id = drive_motor - self.turning_motor_can_id = turning_motor - self.encoder_offset = 0 - self.current_absolute_angle = None - self.gazebo_wheel = None - self.gazebo_swerve = None - self.prev_angle = 0.0 - self.drivetrain = drivetrain - - def set_power(self, power: float) -> None: - self.drivetrain.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.drive_motor_can_id, type="duty_cycle", value=power) - ) - - def set_angle(self, angle: float) -> None: - angle = (360 - angle) % 360 - - self.drivetrain.cli_motor_set.call_async( - MotorCommandSet.Request( - can_id=self.turning_motor_can_id, - type="position", - value=(angle - self.encoder_offset) * self.drivetrain.STEERING_MOTOR_GEAR_RATIO, - ) - ) - - def reset(self, current_relative_angle) -> None: - self.encoder_offset = self.current_absolute_angle - current_relative_angle - self.drivetrain.get_logger().info( - f"CAN ID {self.turning_motor_can_id} Absolute Encoder angle offset set to: {self.encoder_offset}" - ) - self.set_angle(0) # Rotate the module to the 0 degree position - - def set_state(self, power: float, angle: float) -> None: - self.set_angle(angle) - self.set_power(power) - if self.drivetrain.GAZEBO_SIMULATION: - self.publish_gazebo(power, angle) - - def set_gazebo_pubs(self, wheel, swerve): - self.gazebo_wheel = wheel - self.gazebo_swerve = swerve - - def publish_gazebo(self, power: float, angle: float) -> None: - # Convert from counterclockwise -> clockwise - angle = (360 - angle) % 360 - # Convert from degrees to radians - rad = angle * math.pi / 180 - - speed = power * 5 - self.gazebo_wheel.publish(Float64(data=speed)) - self.gazebo_swerve.publish(Float64(data=rad)) - - -# This class represents the drivetrain as a whole (4 swerve modules) class DrivetrainNode(Node): def __init__(self): """Initialize the ROS 2 drivetrain node.""" @@ -82,58 +23,30 @@ def __init__(self): # Define default values for our ROS parameters below # self.declare_parameter("FRONT_LEFT_DRIVE", 10) - self.declare_parameter("FRONT_LEFT_TURN", 4) self.declare_parameter("FRONT_RIGHT_DRIVE", 9) - self.declare_parameter("FRONT_RIGHT_TURN", 3) self.declare_parameter("BACK_LEFT_DRIVE", 7) - self.declare_parameter("BACK_LEFT_TURN", 6) self.declare_parameter("BACK_RIGHT_DRIVE", 8) - self.declare_parameter("BACK_RIGHT_TURN", 4) self.declare_parameter("HALF_WHEEL_BASE", 0.5) self.declare_parameter("HALF_TRACK_WIDTH", 0.5) - self.declare_parameter("STEERING_MOTOR_GEAR_RATIO", 40) - self.declare_parameter("FRONT_LEFT_MAGNET_OFFSET", 97) - self.declare_parameter("FRONT_RIGHT_MAGNET_OFFSET", 873) - self.declare_parameter("BACK_LEFT_MAGNET_OFFSET", 50) - self.declare_parameter("BACK_RIGHT_MAGNET_OFFSET", 647) - self.declare_parameter("ABSOLUTE_ENCODER_COUNTS", 1024) self.declare_parameter("GAZEBO_SIMULATION", False) # Assign the ROS Parameters to member variables below # self.FRONT_LEFT_DRIVE = self.get_parameter("FRONT_LEFT_DRIVE").value - self.FRONT_LEFT_TURN = self.get_parameter("FRONT_LEFT_TURN").value self.FRONT_RIGHT_DRIVE = self.get_parameter("FRONT_RIGHT_DRIVE").value - self.FRONT_RIGHT_TURN = self.get_parameter("FRONT_RIGHT_TURN").value self.BACK_LEFT_DRIVE = self.get_parameter("BACK_LEFT_DRIVE").value - self.BACK_LEFT_TURN = self.get_parameter("BACK_LEFT_TURN").value self.BACK_RIGHT_DRIVE = self.get_parameter("BACK_RIGHT_DRIVE").value - self.BACK_RIGHT_TURN = self.get_parameter("BACK_RIGHT_TURN").value self.HALF_WHEEL_BASE = self.get_parameter("HALF_WHEEL_BASE").value self.HALF_TRACK_WIDTH = self.get_parameter("HALF_TRACK_WIDTH").value - self.STEERING_MOTOR_GEAR_RATIO = self.get_parameter("STEERING_MOTOR_GEAR_RATIO").value - self.FRONT_LEFT_MAGNET_OFFSET = self.get_parameter("FRONT_LEFT_MAGNET_OFFSET").value - self.FRONT_RIGHT_MAGNET_OFFSET = self.get_parameter("FRONT_RIGHT_MAGNET_OFFSET").value - self.BACK_LEFT_MAGNET_OFFSET = self.get_parameter("BACK_LEFT_MAGNET_OFFSET").value - self.BACK_RIGHT_MAGNET_OFFSET = self.get_parameter("BACK_RIGHT_MAGNET_OFFSET").value - self.ABSOLUTE_ENCODER_COUNTS = self.get_parameter("ABSOLUTE_ENCODER_COUNTS").value self.GAZEBO_SIMULATION = self.get_parameter("GAZEBO_SIMULATION").value # Define publishers and subscribers here self.cmd_vel_sub = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_callback, 10) - self.absolute_encoders_sub = self.create_subscription( - AbsoluteEncoders, "absoluteEncoders", self.absolute_encoders_callback, 10 - ) if self.GAZEBO_SIMULATION: self.gazebo_wheel1_pub = self.create_publisher(Float64, "wheel1/cmd_vel", 10) self.gazebo_wheel2_pub = self.create_publisher(Float64, "wheel2/cmd_vel", 10) self.gazebo_wheel3_pub = self.create_publisher(Float64, "wheel3/cmd_vel", 10) self.gazebo_wheel4_pub = self.create_publisher(Float64, "wheel4/cmd_vel", 10) - self.gazebo_swerve1_pub = self.create_publisher(Float64, "swerve1/cmd_pos", 10) - self.gazebo_swerve2_pub = self.create_publisher(Float64, "swerve2/cmd_pos", 10) - self.gazebo_swerve3_pub = self.create_publisher(Float64, "swerve3/cmd_pos", 10) - self.gazebo_swerve4_pub = self.create_publisher(Float64, "swerve4/cmd_pos", 10) - # Define service clients here self.cli_motor_set = self.create_client(MotorCommandSet, "motor/set") self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get") @@ -141,168 +54,56 @@ def __init__(self): # Define services (methods callable from the outside) here self.srv_stop = self.create_service(Stop, "drivetrain/stop", self.stop_callback) self.srv_drive = self.create_service(Drive, "drivetrain/drive", self.drive_callback) - self.srv_calibrate = self.create_service(Stop, "drivetrain/calibrate", self.calibrate_callback) - - # Define timers here - self.absolute_angle_timer = self.create_timer(0.05, self.absolute_angle_reset) # Print the ROS Parameters to the terminal below # self.get_logger().info("FRONT_LEFT_DRIVE has been set to: " + str(self.FRONT_LEFT_DRIVE)) - self.get_logger().info("FRONT_LEFT_TURN has been set to: " + str(self.FRONT_LEFT_TURN)) self.get_logger().info("FRONT_RIGHT_DRIVE has been set to: " + str(self.FRONT_RIGHT_DRIVE)) - self.get_logger().info("FRONT_RIGHT_TURN has been set to: " + str(self.FRONT_RIGHT_TURN)) self.get_logger().info("BACK_LEFT_DRIVE has been set to: " + str(self.BACK_LEFT_DRIVE)) - self.get_logger().info("BACK_LEFT_TURN has been set to: " + str(self.BACK_LEFT_TURN)) self.get_logger().info("BACK_RIGHT_DRIVE has been set to: " + str(self.BACK_RIGHT_DRIVE)) - self.get_logger().info("BACK_RIGHT_TURN has been set to: " + str(self.BACK_RIGHT_TURN)) self.get_logger().info("HALF_WHEEL_BASE has been set to: " + str(self.HALF_WHEEL_BASE)) self.get_logger().info("HALF_TRACK_WIDTH has been set to: " + str(self.HALF_TRACK_WIDTH)) - self.get_logger().info("STEERING_MOTOR_GEAR_RATIO has been set to: " + str(self.STEERING_MOTOR_GEAR_RATIO)) - self.get_logger().info("FRONT_LEFT_MAGNET_OFFSET has been set to: " + str(self.FRONT_LEFT_MAGNET_OFFSET)) - self.get_logger().info("FRONT_RIGHT_MAGNET_OFFSET has been set to: " + str(self.FRONT_RIGHT_MAGNET_OFFSET)) - self.get_logger().info("BACK_LEFT_MAGNET_OFFSET has been set to: " + str(self.BACK_LEFT_MAGNET_OFFSET)) - self.get_logger().info("BACK_RIGHT_MAGNET_OFFSET has been set to: " + str(self.BACK_RIGHT_MAGNET_OFFSET)) - self.get_logger().info("ABSOLUTE_ENCODER_COUNTS has been set to: " + str(self.ABSOLUTE_ENCODER_COUNTS)) self.get_logger().info("GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION)) - # Create each swerve module using - self.front_left = SwerveModule(self.FRONT_LEFT_DRIVE, self.FRONT_LEFT_TURN, self) - self.front_right = SwerveModule(self.FRONT_RIGHT_DRIVE, self.FRONT_RIGHT_TURN, self) - self.back_left = SwerveModule(self.BACK_LEFT_DRIVE, self.BACK_LEFT_TURN, self) - self.back_right = SwerveModule(self.BACK_RIGHT_DRIVE, self.BACK_RIGHT_TURN, self) if self.GAZEBO_SIMULATION: + # TODO: The lines below need to be modified self.front_left.set_gazebo_pubs(self.gazebo_wheel1_pub, self.gazebo_swerve1_pub) self.front_right.set_gazebo_pubs(self.gazebo_wheel3_pub, self.gazebo_swerve3_pub) self.back_left.set_gazebo_pubs(self.gazebo_wheel4_pub, self.gazebo_swerve4_pub) self.back_right.set_gazebo_pubs(self.gazebo_wheel2_pub, self.gazebo_swerve2_pub) - def absolute_angle_reset(self): - # self.front_left was chosen arbitrarily - if self.front_left.current_absolute_angle is not None: - print("Absolute Encoder angles reset") - - # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. - # Divide this by the gear ratio to get the wheel position. - front_left_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.FRONT_LEFT_TURN) - ) - front_left_future.add_done_callback( - lambda future: self.front_left.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO) - ) - - # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. - # Divide this by the gear ratio to get the wheel position. - front_right_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.FRONT_RIGHT_TURN) - ) - front_right_future.add_done_callback( - lambda future: self.front_right.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO) - ) - - # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. - # Divide this by the gear ratio to get the wheel position. - back_left_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.BACK_LEFT_TURN) - ) - back_left_future.add_done_callback( - lambda future: self.back_left.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO) - ) - - # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. - # Divide this by the gear ratio to get the wheel position. - back_right_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.BACK_RIGHT_TURN) - ) - back_right_future.add_done_callback( - lambda future: self.back_right.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO) - ) - - self.absolute_angle_timer.cancel() - + # Define subsystem methods here - def drive(self, forward_power: float, horizontal_power: float, turning_power: float) -> None: - """This method drives the robot with the desired forward, horizontal and turning power.""" + def drive(self, linear_power: float, turning_power: float) -> None: + """This method drives the robot with the desired forward and turning power.""" # reverse turning direction turning_power *= -1 - - # Do not change the angle of the modules if the robot is being told to stop - if abs(forward_power) < 0.05 and abs(horizontal_power) < 0.05 and abs(turning_power) < 0.05: - self.front_left.set_state(0.0, self.front_left.prev_angle) - self.front_right.set_state(0.0, self.front_right.prev_angle) - self.back_left.set_state(0.0, self.back_left.prev_angle) - self.back_right.set_state(0.0, self.back_right.prev_angle) - return - - # Vector layouts = [Drive Power, Drive Direction(Degrees from forwards going counterclockwise)] - # Intermediate equations to simplify future expressions - A = horizontal_power - turning_power * self.HALF_WHEEL_BASE - B = horizontal_power + turning_power * self.HALF_WHEEL_BASE - C = forward_power - turning_power * self.HALF_TRACK_WIDTH - D = forward_power + turning_power * self.HALF_TRACK_WIDTH - - # Gives the desired speed and angle for each module - # Note: Angle has range from 0 to 360 degrees to make future calculations easier - front_left_vector = [math.sqrt(B**2 + D**2), ((math.atan2(B, D) * 180 / math.pi) + 360) % 360] - front_right_vector = [math.sqrt(B**2 + C**2), ((math.atan2(B, C) * 180 / math.pi) + 360) % 360] - back_left_vector = [math.sqrt(A**2 + D**2), ((math.atan2(A, D) * 180 / math.pi) + 360) % 360] - back_right_vector = [math.sqrt(A**2 + C**2), ((math.atan2(A, C) * 180 / math.pi) + 360) % 360] - - # Normalize wheel speeds if necessary - largest_power = max( - [abs(front_left_vector[0]), abs(front_right_vector[0]), abs(back_left_vector[0]), abs(back_right_vector[0])] - ) - if largest_power > 1.0: - front_left_vector[0] = front_left_vector[0] / largest_power - front_right_vector[0] = front_right_vector[0] / largest_power - back_left_vector[0] = back_left_vector[0] / largest_power - back_right_vector[0] = back_right_vector[0] / largest_power - - # Note: no module should ever have to rotate more than 90 degrees from its current angle - if ( - abs(front_left_vector[1] - self.front_left.prev_angle) > 90 - and abs(front_left_vector[1] - self.front_left.prev_angle) < 270 - ): - front_left_vector[1] = (front_left_vector[1] + 180) % 360 - # reverse speed of the module - front_left_vector[0] = front_left_vector[0] * -1 - if ( - abs(front_right_vector[1] - self.front_right.prev_angle) > 90 - and abs(front_right_vector[1] - self.front_right.prev_angle) < 270 - ): - front_right_vector[1] = (front_right_vector[1] + 180) % 360 - # reverse speed of the module - front_right_vector[0] = front_right_vector[0] * -1 - if ( - abs(back_left_vector[1] - self.back_left.prev_angle) > 90 - and abs(back_left_vector[1] - self.back_left.prev_angle) < 270 - ): - back_left_vector[1] = (back_left_vector[1] + 180) % 360 - # reverse speed of the module - back_left_vector[0] = back_left_vector[0] * -1 - if ( - abs(back_right_vector[1] - self.back_right.prev_angle) > 90 - and abs(back_right_vector[1] - self.back_right.prev_angle) < 270 - ): - back_right_vector[1] = (back_right_vector[1] + 180) % 360 - # reverse speed of the module - back_right_vector[0] = back_right_vector[0] * -1 - - self.front_left.set_state(front_left_vector[0], front_left_vector[1]) - self.front_right.set_state(front_right_vector[0], front_right_vector[1]) - self.back_left.set_state(back_left_vector[0], back_left_vector[1]) - self.back_right.set_state(back_right_vector[0], back_right_vector[1]) - - # Update the prev_angle of each module - self.front_left.prev_angle = front_left_vector[1] - self.front_right.prev_angle = front_right_vector[1] - self.back_left.prev_angle = back_left_vector[1] - self.back_right.prev_angle = back_right_vector[1] - + #TODO: check in simulation if we need this ^^^ + + #clamp the values to -1 to 1 + linear_power = max(-1.0, min(linear_power, 1.0)) + turning_power = max(-1.0, min(turning_power, 1.0)) + + + leftPower = linear_power - turning_power + rightPower = linear_power + turning_power + + # Desaturate the wheel speeds if needed + if (math.abs(leftPower) > 1.0 or math.abs(rightPower) > 1.0): + + scale_factor = 1.0/max(leftPower, rightPower) + leftPower *= scale_factor + rightPower *= scale_factor + + MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=leftPower) + MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=leftPower) + MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=rightPower) + MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=rightPower) + def stop(self) -> None: """This method stops the drivetrain.""" - self.drive(0.0, 0.0, 0.0) + self.drive(0.0, 0.0) # Define service callback methods here @@ -314,70 +115,15 @@ def stop_callback(self, request, response): def drive_callback(self, request, response): """This service request drives the robot with the specified speeds.""" - self.drive(request.forward_power, request.horizontal_power, request.turning_power) + self.drive(request.forward_power, request.turning_power) response.success = 0 # indicates success return response - def calibrate_callback(self, request, response): - """This service request calibrates the drivetrain.""" - if self.front_left.current_absolute_angle is not None: - # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. - # Divide this by the gear ratio to get the wheel position. - front_left_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.FRONT_LEFT_TURN) - ) - front_left_future.add_done_callback( - lambda future: self.front_left.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO) - ) - - # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. - # Divide this by the gear ratio to get the wheel position. - front_right_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.FRONT_RIGHT_TURN) - ) - front_right_future.add_done_callback( - lambda future: self.front_right.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO) - ) - - # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. - # Divide this by the gear ratio to get the wheel position. - back_left_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.BACK_LEFT_TURN) - ) - back_left_future.add_done_callback( - lambda future: self.back_left.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO) - ) - - # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. - # Divide this by the gear ratio to get the wheel position. - back_right_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.BACK_RIGHT_TURN) - ) - back_right_future.add_done_callback( - lambda future: self.back_right.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO) - ) - - response.success = 0 # indicates success - else: - response.success = 1 # indicates failure - return response - # Define subscriber callback methods here def cmd_vel_callback(self, msg: Twist) -> None: """This method is called whenever a message is received on the cmd_vel topic.""" - self.drive(msg.linear.y, msg.linear.x, msg.angular.z) - - def absolute_encoders_callback(self, msg: AbsoluteEncoders) -> None: - """This method is called whenever a message is received on the absoluteEncoders topic.""" - front_left_adjusted = (msg.front_left_encoder - self.FRONT_LEFT_MAGNET_OFFSET) % self.ABSOLUTE_ENCODER_COUNTS - front_right_adjusted = (msg.front_right_encoder - self.FRONT_RIGHT_MAGNET_OFFSET) % self.ABSOLUTE_ENCODER_COUNTS - back_left_adjusted = (msg.back_left_encoder - self.BACK_LEFT_MAGNET_OFFSET) % self.ABSOLUTE_ENCODER_COUNTS - back_right_adjusted = (msg.back_right_encoder - self.BACK_RIGHT_MAGNET_OFFSET) % self.ABSOLUTE_ENCODER_COUNTS - self.front_left.current_absolute_angle = 360 * front_left_adjusted / self.ABSOLUTE_ENCODER_COUNTS - self.front_right.current_absolute_angle = 360 * front_right_adjusted / self.ABSOLUTE_ENCODER_COUNTS - self.back_left.current_absolute_angle = 360 * back_left_adjusted / self.ABSOLUTE_ENCODER_COUNTS - self.back_right.current_absolute_angle = 360 * back_right_adjusted / self.ABSOLUTE_ENCODER_COUNTS + self.drive(msg.linear.y, msg.angular.z) def main(args=None): From b4c1b05f272865ca72a23159fbdc539863037775 Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Tue, 24 Sep 2024 20:33:44 -0500 Subject: [PATCH 06/45] Autoformat the drivetrain_node --- src/drivetrain/drivetrain/drivetrain_node.py | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/drivetrain/drivetrain/drivetrain_node.py b/src/drivetrain/drivetrain/drivetrain_node.py index 74874cee..c81c6db3 100644 --- a/src/drivetrain/drivetrain/drivetrain_node.py +++ b/src/drivetrain/drivetrain/drivetrain_node.py @@ -16,6 +16,7 @@ # Import custom ROS 2 interfaces from rovr_interfaces.srv import Stop, Drive, MotorCommandSet, MotorCommandGet + class DrivetrainNode(Node): def __init__(self): """Initialize the ROS 2 drivetrain node.""" @@ -64,7 +65,6 @@ def __init__(self): self.get_logger().info("HALF_TRACK_WIDTH has been set to: " + str(self.HALF_TRACK_WIDTH)) self.get_logger().info("GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION)) - if self.GAZEBO_SIMULATION: # TODO: The lines below need to be modified self.front_left.set_gazebo_pubs(self.gazebo_wheel1_pub, self.gazebo_swerve1_pub) @@ -72,35 +72,33 @@ def __init__(self): self.back_left.set_gazebo_pubs(self.gazebo_wheel4_pub, self.gazebo_swerve4_pub) self.back_right.set_gazebo_pubs(self.gazebo_wheel2_pub, self.gazebo_swerve2_pub) - # Define subsystem methods here def drive(self, linear_power: float, turning_power: float) -> None: """This method drives the robot with the desired forward and turning power.""" # reverse turning direction turning_power *= -1 - #TODO: check in simulation if we need this ^^^ + # TODO: check in simulation if we need this ^^^ - #clamp the values to -1 to 1 + # clamp the values to -1 to 1 linear_power = max(-1.0, min(linear_power, 1.0)) turning_power = max(-1.0, min(turning_power, 1.0)) - leftPower = linear_power - turning_power rightPower = linear_power + turning_power # Desaturate the wheel speeds if needed - if (math.abs(leftPower) > 1.0 or math.abs(rightPower) > 1.0): - - scale_factor = 1.0/max(leftPower, rightPower) + if math.abs(leftPower) > 1.0 or math.abs(rightPower) > 1.0: + + scale_factor = 1.0 / max(leftPower, rightPower) leftPower *= scale_factor rightPower *= scale_factor - + MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=leftPower) MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=leftPower) MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=rightPower) MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=rightPower) - + def stop(self) -> None: """This method stops the drivetrain.""" self.drive(0.0, 0.0) From d0ea543c37622d0c5561e185769691f55fba1ec5 Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Tue, 24 Sep 2024 20:39:41 -0500 Subject: [PATCH 07/45] Fixed number of bytes to read in read_serial --- src/rovr_control/rovr_control/read_serial.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rovr_control/rovr_control/read_serial.py b/src/rovr_control/rovr_control/read_serial.py index adb9d5db..4f0531b5 100644 --- a/src/rovr_control/rovr_control/read_serial.py +++ b/src/rovr_control/rovr_control/read_serial.py @@ -27,8 +27,8 @@ def __init__(self): self.get_logger().fatal("Killing read_serial node") self.destroy_node() return - data = self.arduino.read(10) # Pause until 10 bytes are read - decoded = struct.unpack("??", data) # Use h for each int because arduino int is 2 bytes + data = self.arduino.read(2) # Pause until 2 bytes are read + decoded = struct.unpack("??", data) # Use h for integers because arduino int is 2 bytes, use ? for booleans msg = LimitSwitches() msg.top_limit_switch = decoded[0] From 1cf4c28b7e7c76969cb506d94942d618ef4c6b25 Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Tue, 24 Sep 2024 20:43:04 -0500 Subject: [PATCH 08/45] Minor Whitespace Fixes --- src/motor_control/src/motor_control_node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index c92fad30..e202666e 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -227,12 +227,12 @@ class MotorControlNode : public rclcpp::Node { public: MotorControlNode() : Node("MotorControlNode") { // Define default values for our ROS parameters below # - this->declare_parameter("CAN_INTERFACE_TRANSMIT", "can0"); + this->declare_parameter("CAN_INTERFACE_TRANSMIT", "can0"); this->declare_parameter("CAN_INTERFACE_RECEIVE", "can0"); this->declare_parameter("SKIMMER_LIFT_MOTOR", 1); // Print the ROS Parameters to the terminal below # - RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_TRANSMIT parameter set to: %s", this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string().c_str()); + RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_TRANSMIT parameter set to: %s", this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string().c_str()); RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_RECEIVE parameter set to: %s", this->get_parameter("CAN_INTERFACE_RECEIVE").as_string().c_str()); RCLCPP_INFO(this->get_logger(), "SKIMMER_LIFT_MOTOR parameter set to: %ld", this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()); @@ -243,7 +243,7 @@ class MotorControlNode : public rclcpp::Node { "motor/get", std::bind(&MotorControlNode::get_callback, this, _1, _2)); this->pid_controllers[this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 20, 0.05); - + // Initialize timers below // timer = this->create_wall_timer(500ms, std::bind(&MotorControlNode::timer_callback, this)); From ad4e84cad7c8b78fa0a0ccc1761eee33f7f8f5bb Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Tue, 24 Sep 2024 20:47:50 -0500 Subject: [PATCH 09/45] Fixed the TODO for the Gazebo publishers --- src/drivetrain/drivetrain/drivetrain_node.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/drivetrain/drivetrain/drivetrain_node.py b/src/drivetrain/drivetrain/drivetrain_node.py index c81c6db3..570a8f67 100644 --- a/src/drivetrain/drivetrain/drivetrain_node.py +++ b/src/drivetrain/drivetrain/drivetrain_node.py @@ -48,6 +48,7 @@ def __init__(self): self.gazebo_wheel2_pub = self.create_publisher(Float64, "wheel2/cmd_vel", 10) self.gazebo_wheel3_pub = self.create_publisher(Float64, "wheel3/cmd_vel", 10) self.gazebo_wheel4_pub = self.create_publisher(Float64, "wheel4/cmd_vel", 10) + # Define service clients here self.cli_motor_set = self.create_client(MotorCommandSet, "motor/set") self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get") @@ -65,20 +66,13 @@ def __init__(self): self.get_logger().info("HALF_TRACK_WIDTH has been set to: " + str(self.HALF_TRACK_WIDTH)) self.get_logger().info("GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION)) - if self.GAZEBO_SIMULATION: - # TODO: The lines below need to be modified - self.front_left.set_gazebo_pubs(self.gazebo_wheel1_pub, self.gazebo_swerve1_pub) - self.front_right.set_gazebo_pubs(self.gazebo_wheel3_pub, self.gazebo_swerve3_pub) - self.back_left.set_gazebo_pubs(self.gazebo_wheel4_pub, self.gazebo_swerve4_pub) - self.back_right.set_gazebo_pubs(self.gazebo_wheel2_pub, self.gazebo_swerve2_pub) - # Define subsystem methods here def drive(self, linear_power: float, turning_power: float) -> None: """This method drives the robot with the desired forward and turning power.""" - # reverse turning direction + # Reverse the turning direction turning_power *= -1 - # TODO: check in simulation if we need this ^^^ + # ^^ TODO: Check in simulation if we need this ^^^ # clamp the values to -1 to 1 linear_power = max(-1.0, min(linear_power, 1.0)) @@ -89,7 +83,6 @@ def drive(self, linear_power: float, turning_power: float) -> None: # Desaturate the wheel speeds if needed if math.abs(leftPower) > 1.0 or math.abs(rightPower) > 1.0: - scale_factor = 1.0 / max(leftPower, rightPower) leftPower *= scale_factor rightPower *= scale_factor @@ -99,6 +92,12 @@ def drive(self, linear_power: float, turning_power: float) -> None: MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=rightPower) MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=rightPower) + if self.GAZEBO_SIMULATION: + self.gazebo_wheel1_pub.publish(Float64(data=leftPower)) + self.gazebo_wheel2_pub.publish(Float64(data=leftPower)) + self.gazebo_wheel3_pub.publish(Float64(data=rightPower)) + self.gazebo_wheel4_pub.publish(Float64(data=rightPower)) + def stop(self) -> None: """This method stops the drivetrain.""" self.drive(0.0, 0.0) From 6609ef2f2ba5da097b359521d32f4afd78cf0c9a Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Thu, 26 Sep 2024 15:43:19 -0500 Subject: [PATCH 10/45] Refactor all usages of the drive ROS service --- src/drivetrain/drivetrain/drivetrain_node.py | 16 +++++++++------- src/rovr_control/rovr_control/auto_dig_server.py | 4 +--- .../rovr_control/auto_offload_server.py | 4 +--- .../rovr_control/main_control_node.py | 5 +---- 4 files changed, 12 insertions(+), 17 deletions(-) diff --git a/src/drivetrain/drivetrain/drivetrain_node.py b/src/drivetrain/drivetrain/drivetrain_node.py index 570a8f67..28fe54cc 100644 --- a/src/drivetrain/drivetrain/drivetrain_node.py +++ b/src/drivetrain/drivetrain/drivetrain_node.py @@ -67,19 +67,19 @@ def __init__(self): self.get_logger().info("GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION)) # Define subsystem methods here - def drive(self, linear_power: float, turning_power: float) -> None: + def drive(self, forward_power: float, turning_power: float) -> None: """This method drives the robot with the desired forward and turning power.""" - # Reverse the turning direction + # Reverse the turning direction # TODO: Check in simulation if we need this turning_power *= -1 - # ^^ TODO: Check in simulation if we need this ^^^ - # clamp the values to -1 to 1 - linear_power = max(-1.0, min(linear_power, 1.0)) + # Clamp the values between -1 and 1 + forward_power = max(-1.0, min(forward_power, 1.0)) turning_power = max(-1.0, min(turning_power, 1.0)) - leftPower = linear_power - turning_power - rightPower = linear_power + turning_power + # Calculate the wheel speeds for each side of the drivetrain + leftPower = forward_power - turning_power + rightPower = forward_power + turning_power # Desaturate the wheel speeds if needed if math.abs(leftPower) > 1.0 or math.abs(rightPower) > 1.0: @@ -87,11 +87,13 @@ def drive(self, linear_power: float, turning_power: float) -> None: leftPower *= scale_factor rightPower *= scale_factor + # Send the motor commands to the motor_control_node MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=leftPower) MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=leftPower) MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=rightPower) MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=rightPower) + # Publish the wheel speeds to the gazebo simulation if self.GAZEBO_SIMULATION: self.gazebo_wheel1_pub.publish(Float64(data=leftPower)) self.gazebo_wheel2_pub.publish(Float64(data=leftPower)) diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index fd89ddce..b78fad3f 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -93,9 +93,7 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): # TODO: completing ticket #298 can replace this while loop with a motor ramp up service 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.cli_drivetrain_drive.call_async(Drive.Request(forward_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 self.async_sleep(0.1) # Allows for task to be canceled diff --git a/src/rovr_control/rovr_control/auto_offload_server.py b/src/rovr_control/rovr_control/auto_offload_server.py index bd80e3e9..0ff3ccad 100644 --- a/src/rovr_control/rovr_control/auto_offload_server.py +++ b/src/rovr_control/rovr_control/auto_offload_server.py @@ -66,9 +66,7 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): return result # Drive backward into the berm zone - self.cli_drivetrain_drive.call_async( - Drive.Request(forward_power=0.0, horizontal_power=-0.25, turning_power=0.0) - ) + self.cli_drivetrain_drive.call_async(Drive.Request(forward_power=-0.25, turning_power=0.0)) self.get_logger().info("Auto Driving") # drive for 10 seconds diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 24d80275..2ac5f0b3 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -23,7 +23,7 @@ from action_msgs.msg import GoalStatus # Import custom ROS 2 interfaces -from rovr_interfaces.srv import Stop, Drive, MotorCommandGet, SetPower, SetPosition +from rovr_interfaces.srv import Stop, SetPower, SetPosition from rovr_interfaces.action import CalibrateFieldCoordinates, AutoDig, AutoOffload # Import Python Modules @@ -125,9 +125,6 @@ def __init__(self) -> None: self.cli_skimmer_setPower = self.create_client(SetPower, "skimmer/setPower") self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition") self.cli_drivetrain_stop = self.create_client(Stop, "drivetrain/stop") - self.cli_drivetrain = self.create_client(Drive, "drivetrain/drive") - self.cli_drivetrain_calibrate = self.create_client(Stop, "drivetrain/calibrate") - self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get") self.cli_lift_stop = self.create_client(Stop, "lift/stop") self.cli_lift_zero = self.create_client(Stop, "lift/zero") self.cli_lift_set_power = self.create_client(SetPower, "lift/setPower") From 466a705ea83b04953b96e98f041c0970da17a476 Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Sun, 29 Sep 2024 15:46:38 -0500 Subject: [PATCH 11/45] Remove remaining mentions of absolute encoders --- README.md | 2 +- src/rovr_interfaces/CMakeLists.txt | 1 - src/rovr_interfaces/msg/AbsoluteEncoders.msg | 4 ---- 3 files changed, 1 insertion(+), 6 deletions(-) delete mode 100644 src/rovr_interfaces/msg/AbsoluteEncoders.msg diff --git a/README.md b/README.md index 00d135f1..4e64eae1 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ graph LR P[ZED ROS 2 Wrapper] end D[Arduino Microcontroller] - K[Limit Switches and Absolute Encoders] + K[Limit Switches] E[VESC Motor Controllers] end K --> D diff --git a/src/rovr_interfaces/CMakeLists.txt b/src/rovr_interfaces/CMakeLists.txt index bf38e456..707c238c 100644 --- a/src/rovr_interfaces/CMakeLists.txt +++ b/src/rovr_interfaces/CMakeLists.txt @@ -22,7 +22,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/AutoDig.action" "action/AutoOffload.action" "action/CalibrateFieldCoordinates.action" - "msg/AbsoluteEncoders.msg" "msg/LimitSwitches.msg" "srv/Drive.srv" "srv/MotorCommandGet.srv" diff --git a/src/rovr_interfaces/msg/AbsoluteEncoders.msg b/src/rovr_interfaces/msg/AbsoluteEncoders.msg deleted file mode 100644 index c667741e..00000000 --- a/src/rovr_interfaces/msg/AbsoluteEncoders.msg +++ /dev/null @@ -1,4 +0,0 @@ -int16 front_left_encoder # Reading from the front left absolute encoder -int16 front_right_encoder # Reading from the front right absolute encoder -int16 back_left_encoder # Reading from the back left absolute encoder -int16 back_right_encoder # Reading from the back right absolute encoder \ No newline at end of file From 912db1a942e5813fbad86b303ec430bac5471bfc Mon Sep 17 00:00:00 2001 From: kznhq <145292978+kznhq@users.noreply.github.com> Date: Tue, 1 Oct 2024 23:28:14 +0000 Subject: [PATCH 12/45] changed swerve joints "type" from continuous to fixed --- .../models/master_ASM/urdf/master_ASM.urdf.xacro | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/robot_description/models/master_ASM/urdf/master_ASM.urdf.xacro b/src/robot_description/models/master_ASM/urdf/master_ASM.urdf.xacro index c7ae47fd..26edcdda 100644 --- a/src/robot_description/models/master_ASM/urdf/master_ASM.urdf.xacro +++ b/src/robot_description/models/master_ASM/urdf/master_ASM.urdf.xacro @@ -150,7 +150,7 @@ Stephen Brawner (brawner@gmail.com) + type="fixed"> @@ -246,7 +246,7 @@ Stephen Brawner (brawner@gmail.com) + type="fixed"> @@ -344,7 +344,7 @@ Stephen Brawner (brawner@gmail.com) + type="fixed"> @@ -439,7 +439,7 @@ Stephen Brawner (brawner@gmail.com) + type="fixed"> From c2a1bc4da1640c3e2d4ed64c0fb7d21462afec72 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Tue, 1 Oct 2024 23:57:54 +0000 Subject: [PATCH 13/45] Fixed Nav2 for tank drive --- config/nav2_isaac_sim.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/config/nav2_isaac_sim.yaml b/config/nav2_isaac_sim.yaml index eb1f3af5..e534fe81 100644 --- a/config/nav2_isaac_sim.yaml +++ b/config/nav2_isaac_sim.yaml @@ -106,18 +106,18 @@ controller_server: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True min_vel_x: -0.5 # TODO: adjust this if needed (m/s) - min_vel_y: -0.5 # TODO: adjust this if needed (m/s) + min_vel_y: 0.0 # TODO: adjust this if needed (m/s) max_vel_x: 0.5 # TODO: adjust this if needed (m/s) - max_vel_y: 0.5 # TODO: adjust this if needed (m/s) + max_vel_y: 0.0 # TODO: adjust this if needed (m/s) max_vel_theta: 0.5 # TODO: adjust this if needed (m/s) min_speed_xy: -0.5 # TODO: adjust this if needed (m/s) max_speed_xy: 0.5 # TODO: adjust this if needed (m/s) min_speed_theta: -0.5 # TODO: adjust this if needed (m/s) acc_lim_x: 1.0 # TODO: adjust this if needed (m/s^2) - acc_lim_y: 1.0 # TODO: adjust this if needed (m/s^2) + acc_lim_y: 0.0 # TODO: adjust this if needed (m/s^2) acc_lim_theta: 1.0 # TODO: adjust this if needed (m/s^2) decel_lim_x: -1.0 # TODO: adjust this if needed (m/s^2) - decel_lim_y: -1.0 # TODO: adjust this if needed (m/s^2) + decel_lim_y: 0.0 # TODO: adjust this if needed (m/s^2) decel_lim_theta: -1.0 # TODO: adjust this if needed (m/s^2) vx_samples: 30 vy_samples: 30 @@ -158,12 +158,12 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: false feedback: "OPEN_LOOP" - max_velocity: [1.0, 1.0, 2.0] - min_velocity: [-1.0, -1.0, -2.0] + max_velocity: [1.0, 0.0, 2.0] + min_velocity: [-1.0, 0.0, -2.0] deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 - max_accel: [1.0, 1.0, 2.0] - max_decel: [-1.0, -1.0, -2.0] + max_accel: [1.0, 0.0, 2.0] + max_decel: [-1.0, 0.0, -2.0] odom_topic: "odom" odom_duration: 0.1 From f1e6d55a2466d59f6bd1cd4000ba8084f53063e5 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Wed, 2 Oct 2024 01:23:39 +0000 Subject: [PATCH 14/45] Updated autodig to do in-place digging --- .../rovr_control/auto_dig_server.py | 44 +++++++++---------- 1 file changed, 21 insertions(+), 23 deletions(-) diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index b78fad3f..fdcdc9c3 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -76,40 +76,38 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): # Wait for the lift goal to be reached await self.skimmer_sleep() - # 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 - await self.skimmer_sleep() + # 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: completing ticket #298 can replace this while loop with a motor ramp up service - 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.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 self.async_sleep(0.1) # Allows for task to be canceled - - self.get_logger().info("Auto Driving") - await self.async_sleep(12) # Allows for task to be canceled - self.get_logger().info("Done Driving") + # Lower the skimmer towards the ground slowly + self.cli_lift_setPower.call_async(SetPower.Request(power=-0.5)) + #ADD WAIT TIL skimmer is at bottom, so it reaches the bottom, then digs a bit longer, then moves on + + self.get_logger().info("Auto Digging in Place") + await self.async_sleep(3) # Allows for task to be canceled / reaches the lower limit switch and digs for a while + self.get_logger().info("Done Digging in Place") - # Stop driving and skimming - await self.cli_drivetrain_stop.call_async(Stop.Request()) + # Stop skimming await self.cli_skimmer_stop.call_async(Stop.Request()) self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) + #raise lift to dumping position # Wait for the lift goal to be reached await self.skimmer_sleep() + #dump into receptacle + self.cli_skimmer_setPower.call_async(SetPower.Request(power=-1*(goal_handle.request.skimmer_belt_power))) + + + self.get_logger().info("Dumping into receptacle") + await self.async_sleep(12) # Allows for task to be canceled + self.get_logger().info("Done storing material") + + await self.cli_skimmer_stop.call_async(Stop.Request()) + + self.get_logger().info("Autonomous Digging Procedure Complete!") goal_handle.succeed() return result From 3a2056d9a756b97f664c792a6232019cfeb42a6e Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 3 Oct 2024 23:56:23 +0000 Subject: [PATCH 15/45] small update to autodig to align with new robot design and prepare for asnyc improvements --- src/rovr_control/rovr_control/auto_dig_server.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index fdcdc9c3..44bae786 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -75,7 +75,7 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): self.cli_lift_zero.call_async(Stop.Request()) # Wait for the lift goal to be reached await self.skimmer_sleep() - + #TODO ALL skimmer_sleeps for Lift_zero need to be replaced with awaiting signal from limit switches/awaiting lift_zero # Start the skimmer belt @@ -97,16 +97,6 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): # Wait for the lift goal to be reached await self.skimmer_sleep() - #dump into receptacle - self.cli_skimmer_setPower.call_async(SetPower.Request(power=-1*(goal_handle.request.skimmer_belt_power))) - - - self.get_logger().info("Dumping into receptacle") - await self.async_sleep(12) # Allows for task to be canceled - self.get_logger().info("Done storing material") - - await self.cli_skimmer_stop.call_async(Stop.Request()) - self.get_logger().info("Autonomous Digging Procedure Complete!") goal_handle.succeed() From 7a2fb6c830a2429451e53e0bc54f76f28ed67372 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 3 Oct 2024 21:32:40 -0500 Subject: [PATCH 16/45] finally commiting this garbage. Scuffed fix for Skimmer node, decent code for autodig, and now theres a todo in motor control! --- src/motor_control/src/motor_control_node.cpp | 500 +++++------------- .../rovr_control/auto_dig_server.py | 19 +- src/skimmer/skimmer/skimmer_node.py | 23 +- 3 files changed, 148 insertions(+), 394 deletions(-) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index e202666e..a57b0c9c 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -1,380 +1,120 @@ -// This node publishes CAN bus messages to our VESC brushless motor controllers. -// Original Author: Jude Sauve in 2018 -// Maintainer: Anthony Brogni -// Last Updated: November 2023 - -// Import the ROS 2 Library -#include "rclcpp/rclcpp.hpp" - -// Import ROS 2 Formatted Message Types -#include "can_msgs/msg/frame.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/string.hpp" - -// Import custom ROS 2 interfaces -#include "rovr_interfaces/srv/motor_command_get.hpp" -#include "rovr_interfaces/srv/motor_command_set.hpp" - -// Import Native C++ Libraries -#include -#include -#include -#include -#include -#include // for tuples -#include - -using namespace std::chrono_literals; -using std::placeholders::_1; -using std::placeholders::_2; - -// Calculates the modulus of an input value within a given range. -// If the input is above the maximum input, it wraps around to the minimum input. -// If the input is below the minimum input, it wraps around to the maximum input. -double inputModulus(double input, double minimumInput, double maximumInput) { - double modulus = maximumInput - minimumInput; - - // Wrap input if it's above the maximum input - int numMax = (int) ((input - minimumInput) / modulus); - input -= numMax * modulus; - - // Wrap input if it's below the minimum input - int numMin = (int) ((input - maximumInput) / modulus); - input -= numMin * modulus; - - return input; -} - -// Define a struct to store motor data -struct MotorData { - float dutyCycle; - float velocity; - int tachometer; - std::chrono::time_point timestamp; -}; - -class PIDController { -private: - int COUNTS_PER_REVOLUTION; // How many encoder counts for one 360 degree rotation - int DEAD_BAND; // How close to the target position is close enough - float MAX_POWER; // Cap the power output to the motor (this should be between 0 and 1) - - bool continuous; // Does the input range wrap around (e.g. absolute encoder) - int minimumTachInput, maximumTachInput; // For continuous input, what is the range? - - float kp, ki, kd; - float gravComp; // Gravity compensation constant (if needed) - - int32_t targTach, totalError; - - std::optional prevError; - -public: - bool isActive; - - PIDController(int CountsPerRevolution, float kp, float ki, float kd, float gravComp, int deadband, float maxPower) { - this->COUNTS_PER_REVOLUTION = CountsPerRevolution; - this->DEAD_BAND = deadband; - this->MAX_POWER = maxPower; - - this->kp = kp; - this->ki = ki; - this->kd = kd; - this->gravComp = gravComp; - this->continuous = false; - - this->totalError = 0; - this->isActive = false; - this->prevError = std::nullopt; - } - - float update(int32_t currTach) { - // Calculate the current error - float currError; - if (this->continuous) { - double errorBound = (this->maximumTachInput - this->minimumTachInput) / 2.0; - currError = inputModulus(this->targTach - currTach, -errorBound, errorBound); - } else { - currError = (this->targTach - currTach); - } - - // If the error is within the dead band, return early - if (abs(currError) <= DEAD_BAND) { - return 0; - } - - this->totalError += currError; - - float PIDResult = (currError * this->kp) + (this->totalError * this->ki) + (this->prevError.has_value() ? currError - this->prevError.value() : 0) * this->kd; - - PIDResult = std::clamp(PIDResult, (float)(-this->MAX_POWER), (float)(this->MAX_POWER)); // Clamp the PIDResult to the maximum power - - // Uncomment the line below for debug values: - // std::cout << "Target Tachometer: " << targTach << ", Current Tachometer: " << currTach << ", Current Error: " << currError << ", PIDResult: " << PIDResult << ", Total Error: " << totalError << ", D: " << (this->prevError.has_value() ? currError - this->prevError.value() : 0) << std::endl; - - this->prevError = currError; // Assign the previous error to the current error - - return PIDResult; - } - - void setRotation(float degrees) { - this->isActive = true; - this->targTach = static_cast((degrees / 360.0) * this->COUNTS_PER_REVOLUTION); - this->totalError = 0; - this->prevError = std::nullopt; - } - - int getCountsPerRevolution() { - return this->COUNTS_PER_REVOLUTION; - } - - // Methods for continuous input - void enableContinuousInput(int minDegrees, int maxDegrees) { - this->continuous = true; - this->minimumTachInput = static_cast((minDegrees / 360.0) * this->COUNTS_PER_REVOLUTION); - this->maximumTachInput = static_cast((maxDegrees / 360.0) * this->COUNTS_PER_REVOLUTION); - } - void disableContinuousInput() { - this->continuous = false; - } - bool isContinuousInputEnabled() { - return this->continuous; - } -}; - -class MotorControlNode : public rclcpp::Node { - // Generic method for sending data over the CAN bus - void send_can(uint32_t id, int32_t data) const { - can_msgs::msg::Frame can_msg; // Construct a new CAN message - - can_msg.is_rtr = false; - can_msg.is_error = false; - can_msg.is_extended = true; - - can_msg.id = id; // Set the CAN ID for this message - - can_msg.dlc = 4; // Size of the data array (how many bytes to use, maximum of 8) - can_msg.data[0] = (data >> 24) & 0xFF; // First byte of data - can_msg.data[1] = (data >> 16) & 0xFF; // Second byte of data - can_msg.data[2] = (data >> 8) & 0xFF; // Third byte of data - can_msg.data[3] = data & 0xFF; // Fourth byte of data - - can_pub->publish(can_msg); // Publish the CAN message to the ROS 2 topic - } - - // Set the percent power of the motor between -1.0 and 1.0 - void vesc_set_duty_cycle(uint32_t id, float percentPower) { - // Disable the PID controller for this motor if it's active - if (this->pid_controllers[id]) { - this->pid_controllers[id]->isActive = false; - } - - // Do not allow setting more than 100% power in either direction - percentPower = std::clamp(percentPower, (float)(-1), (float)(1)); - int32_t data = percentPower * 100000; // Convert from percent power to a signed 32-bit integer - - send_can(id + 0x00000000, data); // ID does NOT need to be modified to signify this is a duty cycle command - this->current_msg[id] = std::make_tuple(id + 0x00000000, data); // update the hashmap - RCLCPP_DEBUG(this->get_logger(), "Setting the duty cycle of CAN ID: %u to %f", id, percentPower); // Print Statement - } - - // Set the velocity of the motor in RPM (Rotations Per Minute) - void vesc_set_velocity(uint32_t id, int rpm) { - if (this->pid_controllers[id]) { - this->pid_controllers[id]->isActive = false; - } - int32_t data = rpm; - - send_can(id + 0x00000300, data); // ID must be modified to signify this is a RPM command - this->current_msg[id] = std::make_tuple(id + 0x00000300, data); // update the hashmap - RCLCPP_DEBUG(this->get_logger(), "Setting the velocity of CAN ID: %u to %d RPM", id, rpm); // Print Statement - } - - // Set the position of the motor in degrees - void vesc_set_position(uint32_t id, int position) { - if (this->pid_controllers[id]) { - this->pid_controllers[id]->setRotation(position); - } - RCLCPP_DEBUG(this->get_logger(), "Setting the position of CAN ID: %u to %d degrees", id, position); // Print Statement - } - - // Get the motor controller's current duty cycle command - std::optional vesc_get_duty_cycle(uint32_t id) { - if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { - return this->can_data[id].dutyCycle; - } else { - return std::nullopt; // The data is too stale - } - } - // Get the current velocity of the motor in RPM (Rotations Per Minute) - std::optional vesc_get_velocity(uint32_t id) { - if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { - return this->can_data[id].velocity; - } else { - return std::nullopt; // The data is too stale - } - } - // Get the current position (tachometer reading) of the motor - std::optional vesc_get_position(uint32_t id) { - if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { - return (static_cast(this->can_data[id].tachometer) / static_cast(this->pid_controllers[id]->getCountsPerRevolution())) * 360.0; - } else { - return std::nullopt; // The data is too stale - } - } - -public: - MotorControlNode() : Node("MotorControlNode") { - // Define default values for our ROS parameters below # - this->declare_parameter("CAN_INTERFACE_TRANSMIT", "can0"); - this->declare_parameter("CAN_INTERFACE_RECEIVE", "can0"); - this->declare_parameter("SKIMMER_LIFT_MOTOR", 1); - - // Print the ROS Parameters to the terminal below # - RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_TRANSMIT parameter set to: %s", this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string().c_str()); - RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_RECEIVE parameter set to: %s", this->get_parameter("CAN_INTERFACE_RECEIVE").as_string().c_str()); - RCLCPP_INFO(this->get_logger(), "SKIMMER_LIFT_MOTOR parameter set to: %ld", this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()); - - // Initialize services below // - srv_motor_set = this->create_service( - "motor/set", std::bind(&MotorControlNode::set_callback, this, _1, _2)); - srv_motor_get = this->create_service( - "motor/get", std::bind(&MotorControlNode::get_callback, this, _1, _2)); - - this->pid_controllers[this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 20, 0.05); - - // Initialize timers below // - timer = this->create_wall_timer(500ms, std::bind(&MotorControlNode::timer_callback, this)); - - // Initialize publishers and subscribers below // - // The name of this topic is determined by ros2socketcan_bridge - can_pub = this->create_publisher("CAN/" + this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string() + "/transmit", 100); - // The name of this topic is determined by ros2socketcan_bridge - can_sub = this->create_subscription("CAN/" + this->get_parameter("CAN_INTERFACE_RECEIVE").as_string() + "/receive", 10, std::bind(&MotorControlNode::CAN_callback, this, _1)); - } - -private: - // Continuously send CAN messages to our motor controllers so they don't timeout - void timer_callback() { - // Loop through everything in the current_msg hashmap - for (auto pair : this->current_msg) { - uint32_t motorId = pair.first; - // If the motor controller has previously received a command, send the most recent command again - if ((this->pid_controllers[motorId] && this->pid_controllers[motorId]->isActive == false) || this->pid_controllers[motorId] == NULL) { - send_can(std::get<0>(this->current_msg[motorId]), std::get<1>(this->current_msg[motorId])); - } - } - } - - // Listen for CAN status frames sent by our VESC motor controllers - void CAN_callback(const can_msgs::msg::Frame::SharedPtr can_msg) { - uint32_t motorId = can_msg->id & 0xFF; - uint32_t statusId = (can_msg->id >> 8) & 0xFF; // Packet Status, not frame ID - - // If 'motorId' is not found in the 'can_data' hashmap, add it. - if (this->can_data.count(motorId) == 0) { - this->can_data[motorId] = {0, 0, 0, std::chrono::steady_clock::now()}; - } - - float dutyCycleNow = this->can_data[motorId].dutyCycle; - float RPM = this->can_data[motorId].velocity; - int32_t tachometer = this->can_data[motorId].tachometer; - - switch (statusId) { - case 9: // Packet Status 9 (RPM & Duty Cycle) - RPM = static_cast((can_msg->data[0] << 24) + (can_msg->data[1] << 16) + (can_msg->data[2] << 8) + can_msg->data[3]); - dutyCycleNow = static_cast(((can_msg->data[6] << 8) + can_msg->data[7]) / 10.0); - break; - case 27: // Packet Status 27 (Tachometer) - tachometer = static_cast((can_msg->data[0] << 24) + (can_msg->data[1] << 16) + (can_msg->data[2] << 8) + can_msg->data[3]); - - // Runs the PID controller for this motor if its active - if (this->pid_controllers[motorId] && this->pid_controllers[motorId]->isActive) { - float PIDResult = this->pid_controllers[motorId]->update(tachometer); - - int32_t data = PIDResult * 100000; // Convert from percent power to a signed 32-bit integer - send_can(motorId + 0x00000000, data); // ID does NOT need to be modified to signify this is a duty cycle command - } - - break; - } - - // Store the most recent motor data in the hashmap - this->can_data[motorId] = {dutyCycleNow, RPM, tachometer, std::chrono::steady_clock::now()}; - - RCLCPP_DEBUG(this->get_logger(), "Received status frame %u from CAN ID %u with the following data:", statusId, motorId); - RCLCPP_DEBUG(this->get_logger(), "RPM: %.2f, Duty Cycle: %.2f%%, Tachometer: %d", RPM, dutyCycleNow, tachometer); - } - - // Initialize a hashmap to store the most recent motor data for each CAN ID - std::map can_data; - std::map pid_controllers; - // Adjust this data retention threshold as needed - const std::chrono::seconds threshold = std::chrono::seconds(1); - - // Initialize a hashmap to store the most recent msg for each CAN ID - std::map> current_msg; - - // Callback method for the MotorCommandSet service - void set_callback(const std::shared_ptr request, - std::shared_ptr response) { - - if (strcmp(request->type.c_str(), "velocity") == 0) { - vesc_set_velocity(request->can_id, request->value); - response->success = 0; // indicates success - } else if (strcmp(request->type.c_str(), "duty_cycle") == 0) { - vesc_set_duty_cycle(request->can_id, request->value); - response->success = 0; // indicates success - } else if (strcmp(request->type.c_str(), "position") == 0) { - vesc_set_position(request->can_id, request->value); - response->success = 0; // indicates success - } else { - RCLCPP_ERROR(this->get_logger(), "Unknown motor SET command type: '%s'", request->type.c_str()); - response->success = 1; // indicates failure - } - } - - // Callback method for the MotorCommandGet service - void get_callback(const std::shared_ptr request, - std::shared_ptr response) { - std::optional data = std::nullopt; - - if (strcmp(request->type.c_str(), "velocity") == 0) { - data = vesc_get_velocity(request->can_id); - } else if (strcmp(request->type.c_str(), "duty_cycle") == 0) { - data = vesc_get_duty_cycle(request->can_id); - } else if (strcmp(request->type.c_str(), "position") == 0) { - data = vesc_get_position(request->can_id); - } else { - RCLCPP_ERROR(this->get_logger(), "Unknown motor GET command type: '%s'", request->type.c_str()); - } - - if (data.has_value()) { - response->data = data.value(); - response->success = 0; // indicates success - } else { - response->success = 1; // indicates failure - RCLCPP_WARN(this->get_logger(), "GET command for CAN ID %u read stale data!", request->can_id); - } - } - - rclcpp::TimerBase::SharedPtr timer; - rclcpp::Publisher::SharedPtr can_pub; - rclcpp::Subscription::SharedPtr can_sub; - rclcpp::Service::SharedPtr srv_motor_set; - rclcpp::Service::SharedPtr srv_motor_get; -}; - -// Main method for the node -int main(int argc, char **argv) { - // Initialize ROS 2 - rclcpp::init(argc, argv); - - // Spin the node - rclcpp::spin(std::make_shared()); - - // Free up any resources being used by the node - rclcpp::shutdown(); - return 0; -} +import rclpy +from rclpy.action import ActionServer + +from std_msgs.msg import Bool + +from rovr_interfaces.action import AutoDig +from rovr_interfaces.srv import Drive, Stop, SetPosition, SetPower +from rclpy.action.server import ServerGoalHandle, CancelResponse + +from rovr_control.node_util import AsyncNode + + +class AutoDigServer(AsyncNode): + def __init__(self): + super().__init__("auto_dig_server") + self._action_server = ActionServer( + self, AutoDig, "auto_dig", self.execute_callback, cancel_callback=self.cancel_callback + ) + + # TODO: This should not be needed anymore after ticket #257 is implemented! + self.skimmer_goal_subscription = self.create_subscription( + Bool, "/skimmer/goal_reached", self.skimmer_goal_callback, 10 + ) + + self.cli_drivetrain_drive = self.create_client(Drive, "drivetrain/drive") + self.cli_drivetrain_stop = self.create_client(Stop, "drivetrain/stop") + + self.cli_lift_zero = self.create_client(Stop, "lift/zero") + self.cli_lift_lower = self.create_client(Stop, "lift/lower") + self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition") + self.cli_lift_set_power = self.create_client(SetPower, "lift/setPower") + self.cli_lift_stop = self.create_client(Stop, "lift/stop") + + self.cli_skimmer_stop = self.create_client(Stop, "skimmer/stop") + self.cli_skimmer_setPower = self.create_client(SetPower, "skimmer/setPower") + + async def execute_callback(self, goal_handle: ServerGoalHandle): + 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 + # Wait for the lift goal to be reached + await self.cli_lift_zero.call_async(Stop.Reqest()) + + # Start the skimmer belt + await self.cli_skimmer_setPower.call_async(SetPower.Request(power=goal_handle.request.skimmer_belt_power)) + + # Lower the skimmer towards the ground slowly + await self.cli_lift_lower.call_async(Stop.Reqest()) + + self.get_logger().info("Auto Digging in Place") + await self.async_sleep(3) # Allows for task to be canceled / reaches the lower limit switch and digs for a while + self.get_logger().info("Done Digging in Place") + + # Stop skimming + await self.cli_skimmer_stop.call_async(Stop.Request()) + + await self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) + #raise lift to dumping position + # Wait for the lift goal to be reached + + self.get_logger().info("Autonomous Digging Procedure Complete!") + goal_handle.succeed() + return result + + def cancel_callback(self, cancel_request: ServerGoalHandle): + """This method is called when the action is canceled.""" + self.get_logger().info("Goal is cancelling") + if not self.skimmer_goal_reached.done(): + self.cli_drivetrain_stop.call_async(Stop.Request()) + if super().cancel_callback(cancel_request) == CancelResponse.ACCEPT: + self.cli_drivetrain_stop.call_async(Stop.Request()) + self.cli_skimmer_stop.call_async(Stop.Request()) + return CancelResponse.ACCEPT + + +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_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index 44bae786..79ba734b 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -26,6 +26,7 @@ def __init__(self): self.cli_drivetrain_stop = self.create_client(Stop, "drivetrain/stop") self.cli_lift_zero = self.create_client(Stop, "lift/zero") + self.cli_lift_lower = self.create_client(Stop, "lift/lower") self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition") self.cli_lift_set_power = self.create_client(SetPower, "lift/setPower") self.cli_lift_stop = self.create_client(Stop, "lift/stop") @@ -72,19 +73,15 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): return result # Zero the skimmer - self.cli_lift_zero.call_async(Stop.Request()) # Wait for the lift goal to be reached - await self.skimmer_sleep() - #TODO ALL skimmer_sleeps for Lift_zero need to be replaced with awaiting signal from limit switches/awaiting lift_zero - + await self.cli_lift_zero.call_async(Stop.Reqest()) # Start the skimmer belt - self.cli_skimmer_setPower.call_async(SetPower.Request(power=goal_handle.request.skimmer_belt_power)) + await self.cli_skimmer_setPower.call_async(SetPower.Request(power=goal_handle.request.skimmer_belt_power)) # Lower the skimmer towards the ground slowly - self.cli_lift_setPower.call_async(SetPower.Request(power=-0.5)) - #ADD WAIT TIL skimmer is at bottom, so it reaches the bottom, then digs a bit longer, then moves on - + await self.cli_lift_lower.call_async(Stop.Reqest()) + self.get_logger().info("Auto Digging in Place") await self.async_sleep(3) # Allows for task to be canceled / reaches the lower limit switch and digs for a while self.get_logger().info("Done Digging in Place") @@ -92,11 +89,9 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): # Stop skimming await self.cli_skimmer_stop.call_async(Stop.Request()) - self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) - #raise lift to dumping position + await self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) + #raise lift to dumping position # Wait for the lift goal to be reached - await self.skimmer_sleep() - self.get_logger().info("Autonomous Digging Procedure Complete!") goal_handle.succeed() diff --git a/src/skimmer/skimmer/skimmer_node.py b/src/skimmer/skimmer/skimmer_node.py index 82253acb..832d810b 100644 --- a/src/skimmer/skimmer/skimmer_node.py +++ b/src/skimmer/skimmer/skimmer_node.py @@ -1,7 +1,7 @@ # This ROS 2 node contains code for the skimmer subsystem of the robot. # Original Author: Anthony Brogni in Fall 2023 -# Maintainer: Anthony Brogni -# Last Updated: November 2023 +# Maintainer: Charlie Parece +# Last Updated: October 2024 # Import the ROS 2 Python module import rclpy @@ -33,6 +33,7 @@ def __init__(self): self.srv_lift_stop = self.create_service(Stop, "lift/stop", self.stop_lift_callback) self.srv_lift_set_power = self.create_service(SetPower, "lift/setPower", self.lift_set_power_callback) self.srv_zero_lift = self.create_service(Stop, "lift/zero", self.zero_lift_callback) + self.srv_lower_lift = self.create_service(Stop, "lift/lower", self.lower_lift_callback) # Define publishers here self.publisher_goal_reached = self.create_publisher(Bool, "skimmer/goal_reached", 10) @@ -69,6 +70,8 @@ def __init__(self): self.goal_threshold = 320 # in degrees of the motor # TODO: Tune this threshold if needed # Current state of the lift system self.lift_running = False + # Goal reached internal global + self.goal_reached = False # Limit Switch States self.top_limit_pressed = False @@ -139,6 +142,9 @@ def zero_lift(self) -> None: """This method zeros the lift system by slowly raising it until the top limit switch is pressed.""" self.lift_set_power(0.05) + def lower_lift(self) -> None: + self.lift_set_power(-0.05) + # Define service callback methods here def set_power_callback(self, request, response): """This service request sets power to the skimmer belt.""" @@ -164,6 +170,8 @@ def toggle_callback(self, request, response): def set_position_callback(self, request, response): """This service request sets the position of the lift.""" self.set_position(request.position) + while not self.goal_reached: + pass response.success = 0 # indicates success return response @@ -182,6 +190,16 @@ def lift_set_power_callback(self, request, response): def zero_lift_callback(self, request, response): """This service request zeros the lift system.""" self.zero_lift() + while not self.top_limit_pressed: + pass + response.success = 0 # indicates success + return response + + def lower_lift_callback(self, request, response): + """This service request reverse-zeros the lift system, putting it at the lowest point""" + self.lower_lift() + while not self.bottom_limit_pressed: + pass response.success = 0 # indicates success return response @@ -199,6 +217,7 @@ def done_callback(self, future): <= self.goal_threshold ) self.publisher_goal_reached.publish(goal_reached_msg) + self.goal_reached = goal_reached_msg # Define subscriber callback methods here def limit_switch_callback(self, limit_switches_msg): From dfc071913dac63768a6184929ab0f264ad045b39 Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Thu, 3 Oct 2024 21:54:08 -0500 Subject: [PATCH 17/45] Fixed the motor_control_node (it got messed up in the previous commit) --- src/motor_control/src/motor_control_node.cpp | 500 ++++++++++++++----- 1 file changed, 380 insertions(+), 120 deletions(-) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index a57b0c9c..e202666e 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -1,120 +1,380 @@ -import rclpy -from rclpy.action import ActionServer - -from std_msgs.msg import Bool - -from rovr_interfaces.action import AutoDig -from rovr_interfaces.srv import Drive, Stop, SetPosition, SetPower -from rclpy.action.server import ServerGoalHandle, CancelResponse - -from rovr_control.node_util import AsyncNode - - -class AutoDigServer(AsyncNode): - def __init__(self): - super().__init__("auto_dig_server") - self._action_server = ActionServer( - self, AutoDig, "auto_dig", self.execute_callback, cancel_callback=self.cancel_callback - ) - - # TODO: This should not be needed anymore after ticket #257 is implemented! - self.skimmer_goal_subscription = self.create_subscription( - Bool, "/skimmer/goal_reached", self.skimmer_goal_callback, 10 - ) - - self.cli_drivetrain_drive = self.create_client(Drive, "drivetrain/drive") - self.cli_drivetrain_stop = self.create_client(Stop, "drivetrain/stop") - - self.cli_lift_zero = self.create_client(Stop, "lift/zero") - self.cli_lift_lower = self.create_client(Stop, "lift/lower") - self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition") - self.cli_lift_set_power = self.create_client(SetPower, "lift/setPower") - self.cli_lift_stop = self.create_client(Stop, "lift/stop") - - self.cli_skimmer_stop = self.create_client(Stop, "skimmer/stop") - self.cli_skimmer_setPower = self.create_client(SetPower, "skimmer/setPower") - - async def execute_callback(self, goal_handle: ServerGoalHandle): - 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 - # Wait for the lift goal to be reached - await self.cli_lift_zero.call_async(Stop.Reqest()) - - # Start the skimmer belt - await self.cli_skimmer_setPower.call_async(SetPower.Request(power=goal_handle.request.skimmer_belt_power)) - - # Lower the skimmer towards the ground slowly - await self.cli_lift_lower.call_async(Stop.Reqest()) - - self.get_logger().info("Auto Digging in Place") - await self.async_sleep(3) # Allows for task to be canceled / reaches the lower limit switch and digs for a while - self.get_logger().info("Done Digging in Place") - - # Stop skimming - await self.cli_skimmer_stop.call_async(Stop.Request()) - - await self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) - #raise lift to dumping position - # Wait for the lift goal to be reached - - self.get_logger().info("Autonomous Digging Procedure Complete!") - goal_handle.succeed() - return result - - def cancel_callback(self, cancel_request: ServerGoalHandle): - """This method is called when the action is canceled.""" - self.get_logger().info("Goal is cancelling") - if not self.skimmer_goal_reached.done(): - self.cli_drivetrain_stop.call_async(Stop.Request()) - if super().cancel_callback(cancel_request) == CancelResponse.ACCEPT: - self.cli_drivetrain_stop.call_async(Stop.Request()) - self.cli_skimmer_stop.call_async(Stop.Request()) - return CancelResponse.ACCEPT - - -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() +// This node publishes CAN bus messages to our VESC brushless motor controllers. +// Original Author: Jude Sauve in 2018 +// Maintainer: Anthony Brogni +// Last Updated: November 2023 + +// Import the ROS 2 Library +#include "rclcpp/rclcpp.hpp" + +// Import ROS 2 Formatted Message Types +#include "can_msgs/msg/frame.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/string.hpp" + +// Import custom ROS 2 interfaces +#include "rovr_interfaces/srv/motor_command_get.hpp" +#include "rovr_interfaces/srv/motor_command_set.hpp" + +// Import Native C++ Libraries +#include +#include +#include +#include +#include +#include // for tuples +#include + +using namespace std::chrono_literals; +using std::placeholders::_1; +using std::placeholders::_2; + +// Calculates the modulus of an input value within a given range. +// If the input is above the maximum input, it wraps around to the minimum input. +// If the input is below the minimum input, it wraps around to the maximum input. +double inputModulus(double input, double minimumInput, double maximumInput) { + double modulus = maximumInput - minimumInput; + + // Wrap input if it's above the maximum input + int numMax = (int) ((input - minimumInput) / modulus); + input -= numMax * modulus; + + // Wrap input if it's below the minimum input + int numMin = (int) ((input - maximumInput) / modulus); + input -= numMin * modulus; + + return input; +} + +// Define a struct to store motor data +struct MotorData { + float dutyCycle; + float velocity; + int tachometer; + std::chrono::time_point timestamp; +}; + +class PIDController { +private: + int COUNTS_PER_REVOLUTION; // How many encoder counts for one 360 degree rotation + int DEAD_BAND; // How close to the target position is close enough + float MAX_POWER; // Cap the power output to the motor (this should be between 0 and 1) + + bool continuous; // Does the input range wrap around (e.g. absolute encoder) + int minimumTachInput, maximumTachInput; // For continuous input, what is the range? + + float kp, ki, kd; + float gravComp; // Gravity compensation constant (if needed) + + int32_t targTach, totalError; + + std::optional prevError; + +public: + bool isActive; + + PIDController(int CountsPerRevolution, float kp, float ki, float kd, float gravComp, int deadband, float maxPower) { + this->COUNTS_PER_REVOLUTION = CountsPerRevolution; + this->DEAD_BAND = deadband; + this->MAX_POWER = maxPower; + + this->kp = kp; + this->ki = ki; + this->kd = kd; + this->gravComp = gravComp; + this->continuous = false; + + this->totalError = 0; + this->isActive = false; + this->prevError = std::nullopt; + } + + float update(int32_t currTach) { + // Calculate the current error + float currError; + if (this->continuous) { + double errorBound = (this->maximumTachInput - this->minimumTachInput) / 2.0; + currError = inputModulus(this->targTach - currTach, -errorBound, errorBound); + } else { + currError = (this->targTach - currTach); + } + + // If the error is within the dead band, return early + if (abs(currError) <= DEAD_BAND) { + return 0; + } + + this->totalError += currError; + + float PIDResult = (currError * this->kp) + (this->totalError * this->ki) + (this->prevError.has_value() ? currError - this->prevError.value() : 0) * this->kd; + + PIDResult = std::clamp(PIDResult, (float)(-this->MAX_POWER), (float)(this->MAX_POWER)); // Clamp the PIDResult to the maximum power + + // Uncomment the line below for debug values: + // std::cout << "Target Tachometer: " << targTach << ", Current Tachometer: " << currTach << ", Current Error: " << currError << ", PIDResult: " << PIDResult << ", Total Error: " << totalError << ", D: " << (this->prevError.has_value() ? currError - this->prevError.value() : 0) << std::endl; + + this->prevError = currError; // Assign the previous error to the current error + + return PIDResult; + } + + void setRotation(float degrees) { + this->isActive = true; + this->targTach = static_cast((degrees / 360.0) * this->COUNTS_PER_REVOLUTION); + this->totalError = 0; + this->prevError = std::nullopt; + } + + int getCountsPerRevolution() { + return this->COUNTS_PER_REVOLUTION; + } + + // Methods for continuous input + void enableContinuousInput(int minDegrees, int maxDegrees) { + this->continuous = true; + this->minimumTachInput = static_cast((minDegrees / 360.0) * this->COUNTS_PER_REVOLUTION); + this->maximumTachInput = static_cast((maxDegrees / 360.0) * this->COUNTS_PER_REVOLUTION); + } + void disableContinuousInput() { + this->continuous = false; + } + bool isContinuousInputEnabled() { + return this->continuous; + } +}; + +class MotorControlNode : public rclcpp::Node { + // Generic method for sending data over the CAN bus + void send_can(uint32_t id, int32_t data) const { + can_msgs::msg::Frame can_msg; // Construct a new CAN message + + can_msg.is_rtr = false; + can_msg.is_error = false; + can_msg.is_extended = true; + + can_msg.id = id; // Set the CAN ID for this message + + can_msg.dlc = 4; // Size of the data array (how many bytes to use, maximum of 8) + can_msg.data[0] = (data >> 24) & 0xFF; // First byte of data + can_msg.data[1] = (data >> 16) & 0xFF; // Second byte of data + can_msg.data[2] = (data >> 8) & 0xFF; // Third byte of data + can_msg.data[3] = data & 0xFF; // Fourth byte of data + + can_pub->publish(can_msg); // Publish the CAN message to the ROS 2 topic + } + + // Set the percent power of the motor between -1.0 and 1.0 + void vesc_set_duty_cycle(uint32_t id, float percentPower) { + // Disable the PID controller for this motor if it's active + if (this->pid_controllers[id]) { + this->pid_controllers[id]->isActive = false; + } + + // Do not allow setting more than 100% power in either direction + percentPower = std::clamp(percentPower, (float)(-1), (float)(1)); + int32_t data = percentPower * 100000; // Convert from percent power to a signed 32-bit integer + + send_can(id + 0x00000000, data); // ID does NOT need to be modified to signify this is a duty cycle command + this->current_msg[id] = std::make_tuple(id + 0x00000000, data); // update the hashmap + RCLCPP_DEBUG(this->get_logger(), "Setting the duty cycle of CAN ID: %u to %f", id, percentPower); // Print Statement + } + + // Set the velocity of the motor in RPM (Rotations Per Minute) + void vesc_set_velocity(uint32_t id, int rpm) { + if (this->pid_controllers[id]) { + this->pid_controllers[id]->isActive = false; + } + int32_t data = rpm; + + send_can(id + 0x00000300, data); // ID must be modified to signify this is a RPM command + this->current_msg[id] = std::make_tuple(id + 0x00000300, data); // update the hashmap + RCLCPP_DEBUG(this->get_logger(), "Setting the velocity of CAN ID: %u to %d RPM", id, rpm); // Print Statement + } + + // Set the position of the motor in degrees + void vesc_set_position(uint32_t id, int position) { + if (this->pid_controllers[id]) { + this->pid_controllers[id]->setRotation(position); + } + RCLCPP_DEBUG(this->get_logger(), "Setting the position of CAN ID: %u to %d degrees", id, position); // Print Statement + } + + // Get the motor controller's current duty cycle command + std::optional vesc_get_duty_cycle(uint32_t id) { + if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + return this->can_data[id].dutyCycle; + } else { + return std::nullopt; // The data is too stale + } + } + // Get the current velocity of the motor in RPM (Rotations Per Minute) + std::optional vesc_get_velocity(uint32_t id) { + if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + return this->can_data[id].velocity; + } else { + return std::nullopt; // The data is too stale + } + } + // Get the current position (tachometer reading) of the motor + std::optional vesc_get_position(uint32_t id) { + if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + return (static_cast(this->can_data[id].tachometer) / static_cast(this->pid_controllers[id]->getCountsPerRevolution())) * 360.0; + } else { + return std::nullopt; // The data is too stale + } + } + +public: + MotorControlNode() : Node("MotorControlNode") { + // Define default values for our ROS parameters below # + this->declare_parameter("CAN_INTERFACE_TRANSMIT", "can0"); + this->declare_parameter("CAN_INTERFACE_RECEIVE", "can0"); + this->declare_parameter("SKIMMER_LIFT_MOTOR", 1); + + // Print the ROS Parameters to the terminal below # + RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_TRANSMIT parameter set to: %s", this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string().c_str()); + RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_RECEIVE parameter set to: %s", this->get_parameter("CAN_INTERFACE_RECEIVE").as_string().c_str()); + RCLCPP_INFO(this->get_logger(), "SKIMMER_LIFT_MOTOR parameter set to: %ld", this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()); + + // Initialize services below // + srv_motor_set = this->create_service( + "motor/set", std::bind(&MotorControlNode::set_callback, this, _1, _2)); + srv_motor_get = this->create_service( + "motor/get", std::bind(&MotorControlNode::get_callback, this, _1, _2)); + + this->pid_controllers[this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 20, 0.05); + + // Initialize timers below // + timer = this->create_wall_timer(500ms, std::bind(&MotorControlNode::timer_callback, this)); + + // Initialize publishers and subscribers below // + // The name of this topic is determined by ros2socketcan_bridge + can_pub = this->create_publisher("CAN/" + this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string() + "/transmit", 100); + // The name of this topic is determined by ros2socketcan_bridge + can_sub = this->create_subscription("CAN/" + this->get_parameter("CAN_INTERFACE_RECEIVE").as_string() + "/receive", 10, std::bind(&MotorControlNode::CAN_callback, this, _1)); + } + +private: + // Continuously send CAN messages to our motor controllers so they don't timeout + void timer_callback() { + // Loop through everything in the current_msg hashmap + for (auto pair : this->current_msg) { + uint32_t motorId = pair.first; + // If the motor controller has previously received a command, send the most recent command again + if ((this->pid_controllers[motorId] && this->pid_controllers[motorId]->isActive == false) || this->pid_controllers[motorId] == NULL) { + send_can(std::get<0>(this->current_msg[motorId]), std::get<1>(this->current_msg[motorId])); + } + } + } + + // Listen for CAN status frames sent by our VESC motor controllers + void CAN_callback(const can_msgs::msg::Frame::SharedPtr can_msg) { + uint32_t motorId = can_msg->id & 0xFF; + uint32_t statusId = (can_msg->id >> 8) & 0xFF; // Packet Status, not frame ID + + // If 'motorId' is not found in the 'can_data' hashmap, add it. + if (this->can_data.count(motorId) == 0) { + this->can_data[motorId] = {0, 0, 0, std::chrono::steady_clock::now()}; + } + + float dutyCycleNow = this->can_data[motorId].dutyCycle; + float RPM = this->can_data[motorId].velocity; + int32_t tachometer = this->can_data[motorId].tachometer; + + switch (statusId) { + case 9: // Packet Status 9 (RPM & Duty Cycle) + RPM = static_cast((can_msg->data[0] << 24) + (can_msg->data[1] << 16) + (can_msg->data[2] << 8) + can_msg->data[3]); + dutyCycleNow = static_cast(((can_msg->data[6] << 8) + can_msg->data[7]) / 10.0); + break; + case 27: // Packet Status 27 (Tachometer) + tachometer = static_cast((can_msg->data[0] << 24) + (can_msg->data[1] << 16) + (can_msg->data[2] << 8) + can_msg->data[3]); + + // Runs the PID controller for this motor if its active + if (this->pid_controllers[motorId] && this->pid_controllers[motorId]->isActive) { + float PIDResult = this->pid_controllers[motorId]->update(tachometer); + + int32_t data = PIDResult * 100000; // Convert from percent power to a signed 32-bit integer + send_can(motorId + 0x00000000, data); // ID does NOT need to be modified to signify this is a duty cycle command + } + + break; + } + + // Store the most recent motor data in the hashmap + this->can_data[motorId] = {dutyCycleNow, RPM, tachometer, std::chrono::steady_clock::now()}; + + RCLCPP_DEBUG(this->get_logger(), "Received status frame %u from CAN ID %u with the following data:", statusId, motorId); + RCLCPP_DEBUG(this->get_logger(), "RPM: %.2f, Duty Cycle: %.2f%%, Tachometer: %d", RPM, dutyCycleNow, tachometer); + } + + // Initialize a hashmap to store the most recent motor data for each CAN ID + std::map can_data; + std::map pid_controllers; + // Adjust this data retention threshold as needed + const std::chrono::seconds threshold = std::chrono::seconds(1); + + // Initialize a hashmap to store the most recent msg for each CAN ID + std::map> current_msg; + + // Callback method for the MotorCommandSet service + void set_callback(const std::shared_ptr request, + std::shared_ptr response) { + + if (strcmp(request->type.c_str(), "velocity") == 0) { + vesc_set_velocity(request->can_id, request->value); + response->success = 0; // indicates success + } else if (strcmp(request->type.c_str(), "duty_cycle") == 0) { + vesc_set_duty_cycle(request->can_id, request->value); + response->success = 0; // indicates success + } else if (strcmp(request->type.c_str(), "position") == 0) { + vesc_set_position(request->can_id, request->value); + response->success = 0; // indicates success + } else { + RCLCPP_ERROR(this->get_logger(), "Unknown motor SET command type: '%s'", request->type.c_str()); + response->success = 1; // indicates failure + } + } + + // Callback method for the MotorCommandGet service + void get_callback(const std::shared_ptr request, + std::shared_ptr response) { + std::optional data = std::nullopt; + + if (strcmp(request->type.c_str(), "velocity") == 0) { + data = vesc_get_velocity(request->can_id); + } else if (strcmp(request->type.c_str(), "duty_cycle") == 0) { + data = vesc_get_duty_cycle(request->can_id); + } else if (strcmp(request->type.c_str(), "position") == 0) { + data = vesc_get_position(request->can_id); + } else { + RCLCPP_ERROR(this->get_logger(), "Unknown motor GET command type: '%s'", request->type.c_str()); + } + + if (data.has_value()) { + response->data = data.value(); + response->success = 0; // indicates success + } else { + response->success = 1; // indicates failure + RCLCPP_WARN(this->get_logger(), "GET command for CAN ID %u read stale data!", request->can_id); + } + } + + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr can_pub; + rclcpp::Subscription::SharedPtr can_sub; + rclcpp::Service::SharedPtr srv_motor_set; + rclcpp::Service::SharedPtr srv_motor_get; +}; + +// Main method for the node +int main(int argc, char **argv) { + // Initialize ROS 2 + rclcpp::init(argc, argv); + + // Spin the node + rclcpp::spin(std::make_shared()); + + // Free up any resources being used by the node + rclcpp::shutdown(); + return 0; +} From 2e931878a22756b7bf3ca68f22c36865af1da52c Mon Sep 17 00:00:00 2001 From: cparece1 Date: Wed, 9 Oct 2024 00:44:48 +0000 Subject: [PATCH 18/45] Made set_position calls wait to return until position has been reached --- src/motor_control/src/motor_control_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index e202666e..b01790fd 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -196,6 +196,10 @@ class MotorControlNode : public rclcpp::Node { if (this->pid_controllers[id]) { this->pid_controllers[id]->setRotation(position); } + while( abs(this->can_data[id][2] - this->pid_controllers[id]->targTach) > 100) + { + continue; + } RCLCPP_DEBUG(this->get_logger(), "Setting the position of CAN ID: %u to %d degrees", id, position); // Print Statement } From a058008059d7a08da66b65e62f623b2ee0a9eac8 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 10 Oct 2024 19:14:42 -0500 Subject: [PATCH 19/45] Fixed up set position and set velocity so they dont return til they're done --- src/motor_control/src/motor_control_node.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index 8fbbdd50..9d3623b1 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -188,6 +188,10 @@ class MotorControlNode : public rclcpp::Node { send_can(id + 0x00000300, data); // ID must be modified to signify this is a RPM command this->current_msg[id] = std::make_tuple(id + 0x00000300, data); // update the hashmap + while( rpm != this->can_data[id].velocity) + { + continue; + } RCLCPP_DEBUG(this->get_logger(), "Setting the velocity of CAN ID: %u to %d RPM", id, rpm); // Print Statement } @@ -196,7 +200,7 @@ class MotorControlNode : public rclcpp::Node { if (this->pid_controllers[id]) { this->pid_controllers[id]->setRotation(position); } - while( abs(this->can_data[id][2] - this->pid_controllers[id]->targTach) > 100) + while( abs(this->can_data[id].tachometer - this->pid_controllers[id]->targTach) > 100) { continue; } From 1b4fb4aa1945fd582d33ef13d02ec76b8c253e38 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Tue, 15 Oct 2024 19:42:56 -0500 Subject: [PATCH 20/45] Removed the async node class, and adapted the digger node, and the Auto dig and dump servers. Auto dump requires dump zone be already reached --- src/digger/digger/digger_node.py | 37 +-------- .../rovr_control/auto_dig_server.py | 27 ++---- .../rovr_control/auto_offload_server.py | 82 ++++--------------- src/rovr_control/rovr_control/node_util.py | 47 ----------- 4 files changed, 28 insertions(+), 165 deletions(-) delete mode 100644 src/rovr_control/rovr_control/node_util.py diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index f67150fe..8976586f 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -7,8 +7,6 @@ import rclpy from rclpy.node import Node -# Import ROS 2 formatted message types -from std_msgs.msg import Bool # Import custom ROS 2 interfaces from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet @@ -36,14 +34,10 @@ def __init__(self): self.srv_lower_lift = self.create_service(Stop, "lift/lower", self.lower_lift_callback) # Define publishers here - self.publisher_goal_reached = self.create_publisher(Bool, "digger/goal_reached", 10) # Define subscribers here self.limit_switch_sub = self.create_subscription(LimitSwitches, "limitSwitches", self.limit_switch_callback, 10) - # Define timers here - self.timer = self.create_timer(0.1, self.timer_callback) - # Define default values for our ROS parameters below # self.declare_parameter("DIGGER_BELT_MOTOR", 2) self.declare_parameter("DIGGER_LIFT_MOTOR", 1) @@ -60,18 +54,10 @@ def __init__(self): # Current state of the digger belt self.running = False - # Current goal position (in degrees) - self.current_goal_position = 0 # Current position of the lift motor in degrees self.current_position_degrees = 0 # Relative encoders always initialize to 0 - # Goal Threshold - # if abs(self.current_goal_position - ACTUAL VALUE) <= self.goal_threshold, - # then we should publish True to /digger/goal_reached - self.goal_threshold = 320 # in degrees of the motor # TODO: Tune this threshold if needed # Current state of the lift system self.lift_running = False - # Goal reached internal global - self.goal_reached = False # Limit Switch States self.top_limit_pressed = False @@ -104,15 +90,13 @@ def toggle(self, digger_belt_power: float) -> None: else: self.set_power(digger_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 digger.""" - self.current_goal_position = position # goal position should be in degrees self.cli_motor_set.call_async( MotorCommandSet.Request( type="position", can_id=self.DIGGER_LIFT_MOTOR, - value=float(self.current_goal_position + self.lift_encoder_offset), + value=float(position + self.lift_encoder_offset), ) ) @@ -170,8 +154,7 @@ def toggle_callback(self, request, response): def set_position_callback(self, request, response): """This service request sets the position of the lift.""" self.set_position(request.position) - while not self.goal_reached: - pass + #^ this should already wait due to vesc set position not returning until done response.success = 0 # indicates success return response @@ -203,21 +186,7 @@ def lower_lift_callback(self, request, response): response.success = 0 # indicates success return response - # Define timer callback methods here - def timer_callback(self): - """Publishes whether or not the current goal position has been reached.""" - # This service call will return a future object, that will eventually contain the position in degrees - future = self.cli_motor_get.call_async(MotorCommandGet.Request(type="position", can_id=self.DIGGER_LIFT_MOTOR)) - future.add_done_callback(self.done_callback) - - def done_callback(self, future): - self.current_position_degrees = future.result().data - goal_reached_msg = Bool( - data=abs(self.current_goal_position + self.lift_encoder_offset - self.current_position_degrees) - <= self.goal_threshold - ) - self.publisher_goal_reached.publish(goal_reached_msg) - self.goal_reached = goal_reached_msg +#No more timer callback because setPos works # Define subscriber callback methods here def limit_switch_callback(self, limit_switches_msg): diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index 0a6a561d..df4aee6b 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -1,27 +1,21 @@ import rclpy +from rclpy.node import Node from rclpy.action import ActionServer -from std_msgs.msg import Bool - from rovr_interfaces.action import AutoDig from rovr_interfaces.srv import Drive, Stop, SetPosition, SetPower from rclpy.action.server import ServerGoalHandle, CancelResponse -from rovr_control.node_util import AsyncNode +from asyncio import sleep -class AutoDigServer(AsyncNode): +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 ) - # TODO: This should not be needed anymore after ticket #257 is implemented! - self.digger_goal_subscription = self.create_subscription( - Bool, "/digger/goal_reached", self.digger_goal_callback, 10 - ) - self.cli_drivetrain_drive = self.create_client(Drive, "drivetrain/drive") self.cli_drivetrain_stop = self.create_client(Stop, "drivetrain/stop") @@ -73,7 +67,6 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): return result # Zero the digger - # Wait for the lift goal to be reached await self.cli_lift_zero.call_async(Stop.Reqest()) # Start the digger belt @@ -82,15 +75,15 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): # Lower the digger towards the ground slowly await self.cli_lift_lower.call_async(Stop.Reqest()) - self.get_logger().info("Auto Digging in Place") - await self.async_sleep(3) # Allows for task to be canceled / reaches the lower limit switch and digs for a while + self.get_logger().info("Start of Auto Digging in Place") + await sleep(3) #Stay at lowest pos for 3 seconds while digging self.get_logger().info("Done Digging in Place") # Stop skimming await self.cli_digger_stop.call_async(Stop.Request()) await self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) - #raise lift to dumping position + # raise lift to dumping position # Wait for the lift goal to be reached self.get_logger().info("Autonomous Digging Procedure Complete!") @@ -100,11 +93,9 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): def cancel_callback(self, cancel_request: ServerGoalHandle): """This method is called when the action is canceled.""" self.get_logger().info("Goal is cancelling") - if not self.digger_goal_reached.done(): - self.cli_drivetrain_stop.call_async(Stop.Request()) - if super().cancel_callback(cancel_request) == CancelResponse.ACCEPT: - self.cli_drivetrain_stop.call_async(Stop.Request()) - self.cli_digger_stop.call_async(Stop.Request()) + self.cli_drivetrain_stop.call_async(Stop.Request()) + self.cli_digger_stop.call_async(Stop.Request()) + self.cli_lift_stop.call_async(Stop.Request()) return CancelResponse.ACCEPT diff --git a/src/rovr_control/rovr_control/auto_offload_server.py b/src/rovr_control/rovr_control/auto_offload_server.py index 7590a65b..2e13cb5c 100644 --- a/src/rovr_control/rovr_control/auto_offload_server.py +++ b/src/rovr_control/rovr_control/auto_offload_server.py @@ -1,15 +1,14 @@ import rclpy +from rclpy.node import Node + from rclpy.action import ActionServer -from rclpy.action.server import CancelResponse, ServerGoalHandle, GoalStatus -from std_msgs.msg import Bool +from rclpy.action.server import CancelResponse, ServerGoalHandle from rovr_interfaces.action import AutoOffload -from rovr_interfaces.srv import Drive, Stop, SetPosition, SetPower - -from rovr_control.node_util import AsyncNode +from rovr_interfaces.srv import Stop -class AutoOffloadServer(AsyncNode): +class AutoOffloadServer(Node): def __init__(self): super().__init__("auto_offload_server") self._action_server = ActionServer( @@ -20,19 +19,10 @@ def __init__(self): cancel_callback=self.cancel_callback, ) - # TODO: This should not be needed anymore after ticket #257 is implemented! - self.digger_goal_subscription = self.create_subscription( - Bool, "/digger/goal_reached", self.digger_goal_callback, 10 - ) - self.cli_drivetrain_drive = self.create_client(Drive, "drivetrain/drive") - self.cli_drivetrain_stop = self.create_client(Stop, "drivetrain/stop") - # TODO: This should not be updated to used #257 - self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition") - self.cli_lift_stop = self.create_client(Stop, "lift/stop") + self.cli_dumper_stop = self.create_client(Stop, "dumper/stop") + self.cli_dumper_dump = self.create_client(Stop, "dumper/dump") - self.cli_digger_setPower = self.create_client(SetPower, "digger/setPower") - self.cli_digger_stop = self.create_client(Stop, "digger/stop") async def execute_callback(self, goal_handle: ServerGoalHandle): """This method lays out the procedure for autonomously offloading!""" @@ -40,55 +30,19 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): result = AutoOffload.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") + if not self.cli_dumper_stop.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Dumper stop 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_digger_setPower.wait_for_service(timeout_sec=1.0): - self.get_logger().error("Digger set power service not available") - goal_handle.abort() - return result - if not self.cli_digger_stop.wait_for_service(timeout_sec=1.0): - self.get_logger().error("Digger stop service not available") + if not self.cli_dumper_dump.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Dumper dump service not available") goal_handle.abort() return result - # Drive backward into the berm zone - self.cli_drivetrain_drive.call_async(Drive.Request(forward_power=-0.25, turning_power=0.0)) - - self.get_logger().info("Auto Driving") - # drive for 10 seconds - await self.async_sleep(10) # Allows for task to be canceled - await self.cli_drivetrain_stop.call_async(Stop.Request()) - - # Raise up the digger 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)) - # Wait for the lift goal to be reached - await self.digger_sleep() - if goal_handle.status != GoalStatus.STATUS_CANCELING: - self.get_logger().info("Cancelling") - return - - self.get_logger().info("Offloading") - self.cli_digger_setPower.call_async(SetPower.Request(power=goal_handle.request.digger_belt_power)) - # sleep for the amount of time it takes to offload - time_to_offload = 1.0 / abs(goal_handle.request.digger_belt_power) - await self.async_sleep(time_to_offload) # Allows for task to be canceled - - await self.cli_digger_stop.call_async(Stop.Request()) # Stop the digger belt + + await self.cli_dumper_dump.call_async(Stop.Request()) # Dump Berm + + await self.cli_dumper_stop.call_async(Stop.Request()) # Stop the dumper system self.get_logger().info("Autonomous Offload Procedure Complete!") goal_handle.succeed() return result @@ -97,11 +51,7 @@ def cancel_callback(self, cancel_request: ServerGoalHandle): """This method is called when the action is canceled.""" self.get_logger().info("Goal is cancelling") # If lift is raising stop it - if not self.digger_goal_reached.done(): - self.cli_drivetrain_stop.call_async(Stop.Request()) - if super().cancel_callback(cancel_request) == CancelResponse.ACCEPT: - self.cli_drivetrain_stop.call_async(Stop.Request()) - self.cli_digger_stop.call_async(Stop.Request()) + self.cli_dumper_stop.call_async(Stop.Request()) return CancelResponse.ACCEPT diff --git a/src/rovr_control/rovr_control/node_util.py b/src/rovr_control/rovr_control/node_util.py deleted file mode 100644 index 60cf1e48..00000000 --- a/src/rovr_control/rovr_control/node_util.py +++ /dev/null @@ -1,47 +0,0 @@ -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.digger_goal_reached = Future() - self.digger_goal_reached.set_result(None) - - self.sleep_goal_reached = Future() - self.sleep_goal_reached.set_result(None) - - self.timer = None - - # TODO: This should not be needed anymore after ticket #257 is implemented! - def digger_goal_callback(self, msg: Bool) -> None: - """Update the member variable accordingly.""" - if msg.data: - self.digger_goal_reached.set_result(None) - - # TODO: This should not be needed anymore after ticket #257 is implemented! - async def digger_sleep(self) -> None: - self.digger_goal_reached = Future() - await self.digger_goal_reached - - # 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 From 8b3fa3045f708aab6e44076eb88a24a4daefb2de Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 17 Oct 2024 19:29:09 -0500 Subject: [PATCH 21/45] Changed logic for extend/retract dumper, reread AutoDig, AutoOff, Dumper, and Digger, and all look good. Testing time! --- src/digger/digger/digger_node.py | 3 --- src/dumper/dumper/dumper_node.py | 6 ++++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 8976586f..c27f24f3 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -148,9 +148,6 @@ 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) diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index df32fdcb..c2bffda9 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -95,8 +95,9 @@ def toggle_callback(self, request, response): return response def extend_dumper(self) -> None: + self.set_power(self.DUMPER_POWER) while not self.top_limit_pressed: - self.set_power(self.DUMPER_POWER) + pass self.stop() def extend_callback(self, request, response): @@ -106,8 +107,9 @@ def extend_callback(self, request, response): return response def retract_dumper(self) -> None: + self.set_power(-self.DUMPER_POWER) while not self.bottom_limit_pressed: - self.set_power(-self.DUMPER_POWER) + pass self.stop() def retract_callback(self, request, response): From dfff57b0daea4a8cc0629ce9f71d8f89a6834607 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Fri, 18 Oct 2024 01:05:21 +0000 Subject: [PATCH 22/45] TargTach was private so not accessable, added a getter to work around this. Now up to motor_control_node builds successfull! --- src/motor_control/src/motor_control_node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index 9d3623b1..3e33cc7b 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -141,6 +141,9 @@ class PIDController { bool isContinuousInputEnabled() { return this->continuous; } + int32_t getTargTach() { + return this->targTach; + } }; class MotorControlNode : public rclcpp::Node { @@ -200,7 +203,7 @@ class MotorControlNode : public rclcpp::Node { if (this->pid_controllers[id]) { this->pid_controllers[id]->setRotation(position); } - while( abs(this->can_data[id].tachometer - this->pid_controllers[id]->targTach) > 100) + while( abs(this->can_data[id].tachometer - this->pid_controllers[id]->getTargTach()) > 100) { continue; } From 74f651caa6dd7d33ea3536cff1ffd6cdac2d85ba Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 17 Oct 2024 20:07:54 -0500 Subject: [PATCH 23/45] Motor_control_node now builds --- src/motor_control/src/motor_control_node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index 9d3623b1..3e33cc7b 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -141,6 +141,9 @@ class PIDController { bool isContinuousInputEnabled() { return this->continuous; } + int32_t getTargTach() { + return this->targTach; + } }; class MotorControlNode : public rclcpp::Node { @@ -200,7 +203,7 @@ class MotorControlNode : public rclcpp::Node { if (this->pid_controllers[id]) { this->pid_controllers[id]->setRotation(position); } - while( abs(this->can_data[id].tachometer - this->pid_controllers[id]->targTach) > 100) + while( abs(this->can_data[id].tachometer - this->pid_controllers[id]->getTargTach()) > 100) { continue; } From cc8ee93fe58ccfce72f0c1e662e7711ecf2ffea3 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Tue, 22 Oct 2024 19:39:10 -0500 Subject: [PATCH 24/45] Digger_node should be good, motor control has thresholds, but still relies on while loops- needs to switch to futures/events --- src/digger/digger/digger_node.py | 2 +- src/motor_control/src/motor_control_node.cpp | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index c27f24f3..289ee829 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -98,7 +98,7 @@ def set_position(self, position: int) -> None: can_id=self.DIGGER_LIFT_MOTOR, value=float(position + self.lift_encoder_offset), ) - ) + ).result() def stop_lift(self) -> None: """This method stops the lift.""" diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index 3e33cc7b..b5dd978c 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -24,6 +24,7 @@ #include #include // for tuples #include +#include using namespace std::chrono_literals; using std::placeholders::_1; @@ -188,10 +189,10 @@ class MotorControlNode : public rclcpp::Node { this->pid_controllers[id]->isActive = false; } int32_t data = rpm; - + send_can(id + 0x00000300, data); // ID must be modified to signify this is a RPM command this->current_msg[id] = std::make_tuple(id + 0x00000300, data); // update the hashmap - while( rpm != this->can_data[id].velocity) + while( std::abs(rpm-this->can_data[id].velocity) >= this->velocityThreshold) { continue; } @@ -203,7 +204,7 @@ class MotorControlNode : public rclcpp::Node { if (this->pid_controllers[id]) { this->pid_controllers[id]->setRotation(position); } - while( abs(this->can_data[id].tachometer - this->pid_controllers[id]->getTargTach()) > 100) + while( abs(this->can_data[id].tachometer - this->pid_controllers[id]->getTargTach()) > this->tachThreshold) { continue; } @@ -277,6 +278,10 @@ class MotorControlNode : public rclcpp::Node { } } } + int velocityThreshold = 50; + //TODO: Tune this value + int tachThreshold = 100; + //TODO: Tune this value // Listen for CAN status frames sent by our VESC motor controllers void CAN_callback(const can_msgs::msg::Frame::SharedPtr can_msg) { From f7c39f7f7fe95f732743ff9e009c152a73f124e2 Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Wed, 23 Oct 2024 10:28:09 -0500 Subject: [PATCH 25/45] Auto Formatting --- src/digger/digger/digger_node.py | 4 ++-- src/rovr_control/rovr_control/auto_dig_server.py | 6 ++++-- src/rovr_control/rovr_control/auto_offload_server.py | 3 --- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 289ee829..7914ce23 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -151,7 +151,7 @@ def toggle_callback(self, request, response): def set_position_callback(self, request, response): """This service request sets the position of the lift.""" self.set_position(request.position) - #^ this should already wait due to vesc set position not returning until done + # ^ this should already wait due to vesc set position not returning until done response.success = 0 # indicates success return response @@ -183,7 +183,7 @@ def lower_lift_callback(self, request, response): response.success = 0 # indicates success return response -#No more timer callback because setPos works + # No more timer callback because setPos works # Define subscriber callback methods here def limit_switch_callback(self, limit_switches_msg): diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index df4aee6b..7f6edb0b 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -76,13 +76,15 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): await self.cli_lift_lower.call_async(Stop.Reqest()) self.get_logger().info("Start of Auto Digging in Place") - await sleep(3) #Stay at lowest pos for 3 seconds while digging + await sleep(3) # Stay at lowest pos for 3 seconds while digging self.get_logger().info("Done Digging in Place") # Stop skimming await self.cli_digger_stop.call_async(Stop.Request()) - await self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position)) + await self.cli_lift_setPosition.call_async( + SetPosition.Request(position=goal_handle.request.lift_dumping_position) + ) # raise lift to dumping position # Wait for the lift goal to be reached diff --git a/src/rovr_control/rovr_control/auto_offload_server.py b/src/rovr_control/rovr_control/auto_offload_server.py index 2e13cb5c..ab30e0bc 100644 --- a/src/rovr_control/rovr_control/auto_offload_server.py +++ b/src/rovr_control/rovr_control/auto_offload_server.py @@ -19,11 +19,9 @@ def __init__(self): cancel_callback=self.cancel_callback, ) - self.cli_dumper_stop = self.create_client(Stop, "dumper/stop") self.cli_dumper_dump = self.create_client(Stop, "dumper/dump") - async def execute_callback(self, goal_handle: ServerGoalHandle): """This method lays out the procedure for autonomously offloading!""" self.get_logger().info("Starting Autonomous Offload Procedure!") @@ -39,7 +37,6 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): goal_handle.abort() return result - await self.cli_dumper_dump.call_async(Stop.Request()) # Dump Berm await self.cli_dumper_stop.call_async(Stop.Request()) # Stop the dumper system From 48fe448a09e595f35272fb3fafc0bf7e27e700e2 Mon Sep 17 00:00:00 2001 From: Isopod00 Date: Wed, 23 Oct 2024 10:35:04 -0500 Subject: [PATCH 26/45] velocityThreshold and tachThreshold cleanup --- src/motor_control/src/motor_control_node.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index b5dd978c..7e8c4d61 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -192,7 +192,7 @@ class MotorControlNode : public rclcpp::Node { send_can(id + 0x00000300, data); // ID must be modified to signify this is a RPM command this->current_msg[id] = std::make_tuple(id + 0x00000300, data); // update the hashmap - while( std::abs(rpm-this->can_data[id].velocity) >= this->velocityThreshold) + while(abs(rpm-this->can_data[id].velocity - rpm) > this->velocityThreshold) { continue; } @@ -204,7 +204,7 @@ class MotorControlNode : public rclcpp::Node { if (this->pid_controllers[id]) { this->pid_controllers[id]->setRotation(position); } - while( abs(this->can_data[id].tachometer - this->pid_controllers[id]->getTargTach()) > this->tachThreshold) + while(abs(this->can_data[id].tachometer - this->pid_controllers[id]->getTargTach()) > this->tachThreshold) { continue; } @@ -278,10 +278,9 @@ class MotorControlNode : public rclcpp::Node { } } } - int velocityThreshold = 50; - //TODO: Tune this value - int tachThreshold = 100; - //TODO: Tune this value + + int velocityThreshold = 50; // TODO: Tune this value + int tachThreshold = 100; // TODO: Tune this value // Listen for CAN status frames sent by our VESC motor controllers void CAN_callback(const can_msgs::msg::Frame::SharedPtr can_msg) { From b17b67e4fe92a5b826a0ce1b9f7a9d4ca46ef878 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 29 Oct 2024 19:13:59 -0500 Subject: [PATCH 27/45] Add async await syntax for set_position --- src/digger/digger/digger_node.py | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index a400c6fb..d6a9498b 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -91,15 +91,6 @@ def toggle(self, digger_belt_power: float) -> None: else: self.set_power(digger_belt_power) - def set_position(self, position: int) -> None: - """This method sets the position (in degrees) of the digger.""" - self.cli_motor_set.call_async( - MotorCommandSet.Request( - type="position", - can_id=self.DIGGER_LIFT_MOTOR, - value=float(position + self.lift_encoder_offset), - ) - ).result() def stop_lift(self) -> None: """This method stops the lift.""" @@ -149,10 +140,16 @@ def toggle_callback(self, request, response): response.success = 0 # indicates success return response - def set_position_callback(self, request, response): + async def set_position_callback(self, request, response): """This service request sets the position of the lift.""" - self.set_position(request.position) - # ^ this should already wait due to vesc set position not returning until done + """This method sets the position (in degrees) of the digger.""" + await self.cli_motor_set.call_async( + MotorCommandSet.Request( + type="position", + can_id=self.DIGGER_LIFT_MOTOR, + value=float(request.position + self.lift_encoder_offset), + ) + ) response.success = 0 # indicates success return response From b52316c66585f69d5f267c3de4ec45e75ad8a67d Mon Sep 17 00:00:00 2001 From: cparece1 Date: Tue, 12 Nov 2024 19:39:17 -0600 Subject: [PATCH 28/45] 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 | 2 ++ src/dumper/dumper/dumper_node.py | 4 ++- src/rovr_control/launch/main_launch.py | 2 +- .../launch/main_no_joysticks_launch.py | 2 +- .../rovr_control/auto_dig_server.py | 10 +++--- src/rovr_control/rovr_control/node_util.py | 33 +++++++++++++++++++ 6 files changed, 46 insertions(+), 7 deletions(-) create mode 100644 src/rovr_control/rovr_control/node_util.py diff --git a/config/rovr_control.yaml b/config/rovr_control.yaml index 945285e3..9d3db8bc 100644 --- a/config/rovr_control.yaml +++ b/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 diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index 6cbb2cdf..5b251732 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/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() diff --git a/src/rovr_control/launch/main_launch.py b/src/rovr_control/launch/main_launch.py index 68f5b822..d7134507 100644 --- a/src/rovr_control/launch/main_launch.py +++ b/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", ) diff --git a/src/rovr_control/launch/main_no_joysticks_launch.py b/src/rovr_control/launch/main_no_joysticks_launch.py index b5fae349..8905c63b 100644 --- a/src/rovr_control/launch/main_no_joysticks_launch.py +++ b/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", ) diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index a5fab266..d8673a6b 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -1,5 +1,6 @@ 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 @@ -7,10 +8,8 @@ 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 diff --git a/src/rovr_control/rovr_control/node_util.py b/src/rovr_control/rovr_control/node_util.py new file mode 100644 index 00000000..be5e67ed --- /dev/null +++ b/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 \ No newline at end of file From 3623d28e92cd284af449db6df6761b24c9a5fcdc Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 14 Nov 2024 18:32:53 -0600 Subject: [PATCH 29/45] Added workaround to make dumping berm cancellable (dump is now a series of calls from autooffload server) --- src/digger/digger/digger_node.py | 4 ++-- src/dumper/dumper/dumper_node.py | 6 +++--- .../rovr_control/auto_offload_server.py | 16 ++++++++++++---- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index e34ff015..e8b324cf 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -167,7 +167,7 @@ def lift_set_power_callback(self, request, response): def zero_lift_callback(self, request, response): """This service request zeros the lift system.""" self.zero_lift() - while not self.top_limit_pressed: + while not self.top_limit_pressed and self.running: pass response.success = True return response @@ -175,7 +175,7 @@ def zero_lift_callback(self, request, response): def lower_lift_callback(self, request, response): """This service request reverse-zeros the lift system, putting it at the lowest point""" self.lower_lift() - while not self.bottom_limit_pressed: + while not self.bottom_limit_pressed and self.running: pass response.success = True return response diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index 5b251732..861e0287 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -98,7 +98,7 @@ def toggle_callback(self, request, response): def extend_dumper(self) -> None: self.set_power(self.DUMPER_POWER) - while not self.top_limit_pressed: + while not self.top_limit_pressed and self.running: pass self.stop() @@ -110,7 +110,7 @@ def extend_callback(self, request, response): def retract_dumper(self) -> None: self.set_power(-self.DUMPER_POWER) - while not self.bottom_limit_pressed: + while not self.bottom_limit_pressed and self.running: pass self.stop() @@ -119,7 +119,7 @@ def retract_callback(self, request, response): self.retract_dumper() response.success = True return response - + # NOTE: Dump is no longer used because time.sleep is not cancellable! see Auto_offload_server for dump implementation workaround def dump(self) -> None: # extend the dumper self.extend_dumper() diff --git a/src/rovr_control/rovr_control/auto_offload_server.py b/src/rovr_control/rovr_control/auto_offload_server.py index cbdbccd4..1bdc0f16 100644 --- a/src/rovr_control/rovr_control/auto_offload_server.py +++ b/src/rovr_control/rovr_control/auto_offload_server.py @@ -8,7 +8,7 @@ from std_srvs.srv import Trigger -class AutoOffloadServer(Node): +class AutoOffloadServer(AsyncNode): def __init__(self): super().__init__("auto_offload_server") self._action_server = ActionServer( @@ -20,7 +20,13 @@ def __init__(self): ) self.cli_dumper_stop = self.create_client(Trigger, "dumper/stop") - self.cli_dumper_dump = self.create_client(Trigger, "dumper/dump") + #self.cli_dumper_dump = self.create_client(Trigger, "dumper/dump") + self.cli_dumper_extend = self.create_client(Trigger, "dumper/extendDumper") + self.cli_dumper_retract = self.create_client(Trigger, "dumper/retractDumper") + + self.declare_parameter("dump_time", 5) + self.dumpTime = self.get_parameter("dump_time").value + async def execute_callback(self, goal_handle: ServerGoalHandle): """This method lays out the procedure for autonomously offloading!""" @@ -37,9 +43,11 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): goal_handle.abort() return result - await self.cli_dumper_dump.call_async(Trigger.Request()) # Dump Berm + await self.cli_dumper_extend.call_async(Trigger.Request()) # Dump Berm + + await self.async_sleep(self.dumpTime) - await self.cli_dumper_stop.call_async(Trigger.Request()) # Stop the dumper system + await self.cli_dumper_retract.call_async(Trigger.Request()) # Stop the dumper system self.get_logger().info("Autonomous Offload Procedure Complete!") goal_handle.succeed() return result From b6fd434baf411933be0b0d3879debf1a4285d8e0 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 14 Nov 2024 18:43:22 -0600 Subject: [PATCH 30/45] Switched method used for raising lift --- src/rovr_control/rovr_control/auto_dig_server.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index d8673a6b..b13c533f 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -85,10 +85,8 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): # Stop skimming await self.cli_digger_stop.call_async(Trigger.Request()) - await self.cli_lift_setPosition.call_async( - SetPosition.Request(position=goal_handle.request.lift_dumping_position) - ) # raise lift to dumping position + await self.cli_lift_zero.call_async(Trigger.Reqest()) # Wait for the lift goal to be reached self.get_logger().info("Autonomous Digging Procedure Complete!") From 3741a4206c3f048745d9a42fbd6837b2fb2629ab Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 14 Nov 2024 19:09:13 -0600 Subject: [PATCH 31/45] dumper and digger now use events for limit switches --- src/digger/digger/digger_node.py | 23 ++++++++++++++++++----- src/dumper/dumper/dumper_node.py | 25 +++++++++++++++++++------ 2 files changed, 37 insertions(+), 11 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index e8b324cf..1200d6cb 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -13,7 +13,7 @@ from rovr_interfaces.srv import SetPower, SetPosition from rovr_interfaces.msg import LimitSwitches from std_srvs.srv import Trigger - +import event class DiggerNode(Node): def __init__(self): @@ -63,6 +63,9 @@ def __init__(self): # Limit Switch States self.top_limit_pressed = False self.bottom_limit_pressed = False + self.top_limit_event = event.Event() + self.bottom_limit_event = event.Event() + # Maximum value of the lift motor encoder (bottom of the lift system) IN DEGREES self.MAX_ENCODER_DEGREES = ( @@ -166,18 +169,24 @@ def lift_set_power_callback(self, request, response): def zero_lift_callback(self, request, response): """This service request zeros the lift system.""" + self.top_limit_event.clear() self.zero_lift() - while not self.top_limit_pressed and self.running: - pass + self.top_limit_event.wait() + #while not self.top_limit_pressed and self.running: + #pass response.success = True + self.top_limit_event.clear() return response def lower_lift_callback(self, request, response): """This service request reverse-zeros the lift system, putting it at the lowest point""" + self.bottom_limit_event.clear() self.lower_lift() - while not self.bottom_limit_pressed and self.running: - pass + self.bottom_limit_event.wait() + #while not self.bottom_limit_pressed and self.running: + #pass response.success = True + self.bottom_limit_event.clear() return response # No more timer callback because setPos works @@ -192,10 +201,14 @@ def limit_switch_callback(self, limit_switches_msg): self.top_limit_pressed = limit_switches_msg.top_limit_switch self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch if self.top_limit_pressed: # If the top limit switch is pressed + self.top_limit_event.set() + self.bottom_limit_event.clear() self.lift_encoder_offset = self.current_position_degrees self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) elif self.bottom_limit_pressed: # If the bottom limit switch is pressed + self.bottom_limit_event.set() + self.top_limit_event.clear() self.lift_encoder_offset = self.current_position_degrees - self.MAX_ENCODER_DEGREES self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index 861e0287..7e0565d3 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -12,7 +12,7 @@ from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet, SetPower from rovr_interfaces.msg import LimitSwitches from std_srvs.srv import Trigger - +import event class DumperNode(Node): def __init__(self): @@ -46,7 +46,8 @@ def __init__(self): # Current state of the dumper self.running = False - + self.top_limit_event = event.Event() + self.bottom_limit_event = event.Event() self.limit_switch_sub = self.create_subscription(LimitSwitches, "limitSwitches", self.limit_switch_callback, 10) # Define subsystem methods here @@ -97,9 +98,12 @@ def toggle_callback(self, request, response): return response def extend_dumper(self) -> None: + self.top_limit_event.clear() self.set_power(self.DUMPER_POWER) - while not self.top_limit_pressed and self.running: - pass + self.top_limit_event.wait() + #while not self.top_limit_pressed and self.running: + #pass + self.top_limit_event.clear() self.stop() def extend_callback(self, request, response): @@ -109,9 +113,12 @@ def extend_callback(self, request, response): return response def retract_dumper(self) -> None: + self.bottom_limit_event.clear() self.set_power(-self.DUMPER_POWER) - while not self.bottom_limit_pressed and self.running: - pass + self.bottom_limit_event.wait() + #while not self.bottom_limit_pressed and self.running: + #pass + self.bottom_limit_event.clear() self.stop() def retract_callback(self, request, response): @@ -140,7 +147,13 @@ def limit_switch_callback(self, msg: LimitSwitches): if not self.bottom_limit_pressed and msg.bottom_limit_switch: self.stop() # Stop the lift system self.top_limit_pressed = msg.top_limit_switch + if(self.top_limit_pressed): + self.top_limit_event.set() + self.bottom_limit_event.clear() self.bottom_limit_pressed = msg.bottom_limit_switch + if(self.bottom_limit_pressed): + self.bottom_limit_event.set() + self.top_limit_event.clear() def main(args=None): From 82c01be17a322f9287718d3696645e2205e8805e Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 14 Nov 2024 19:21:46 -0600 Subject: [PATCH 32/45] fixed event import and instantiation --- src/digger/digger/digger_node.py | 6 +++--- src/dumper/dumper/dumper_node.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 1200d6cb..772ccb71 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -13,7 +13,7 @@ from rovr_interfaces.srv import SetPower, SetPosition from rovr_interfaces.msg import LimitSwitches from std_srvs.srv import Trigger -import event +from threading import Event, Thread class DiggerNode(Node): def __init__(self): @@ -63,8 +63,8 @@ def __init__(self): # Limit Switch States self.top_limit_pressed = False self.bottom_limit_pressed = False - self.top_limit_event = event.Event() - self.bottom_limit_event = event.Event() + self.top_limit_event = Event() + self.bottom_limit_event = Event() # Maximum value of the lift motor encoder (bottom of the lift system) IN DEGREES diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index 7e0565d3..bf041e6f 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -12,7 +12,7 @@ from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet, SetPower from rovr_interfaces.msg import LimitSwitches from std_srvs.srv import Trigger -import event +from threading import Event, Thread class DumperNode(Node): def __init__(self): @@ -46,8 +46,8 @@ def __init__(self): # Current state of the dumper self.running = False - self.top_limit_event = event.Event() - self.bottom_limit_event = event.Event() + self.top_limit_event = Event() + self.bottom_limit_event = Event() self.limit_switch_sub = self.create_subscription(LimitSwitches, "limitSwitches", self.limit_switch_callback, 10) # Define subsystem methods here From e05dd76c28088991a663a305cd231c546c93568f Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 14 Nov 2024 19:25:22 -0600 Subject: [PATCH 33/45] made stop also set events --- src/digger/digger/digger_node.py | 2 ++ src/dumper/dumper/dumper_node.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 772ccb71..5e16dc51 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -97,6 +97,8 @@ def toggle(self, digger_belt_power: float) -> None: def stop_lift(self) -> None: """This method stops the lift.""" self.lift_running = False + self.top_limit_event.set() + self.bottom_limit_event.set() self.cli_motor_set.call_async( MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=0.0) ) diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index bf041e6f..0c7947c0 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -69,6 +69,8 @@ def set_power(self, dumper_power: float) -> None: def stop(self) -> None: """This method stops the dumper.""" self.running = False + self.top_limit_event.set() + self.bottom_limit_event.set() self.cli_motor_set.call_async(MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=0.0)) def toggle(self, dumper_power: float) -> None: From e95ff0fabdeb67f6cbc86925fc79e02b1d22cbbd Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 21 Nov 2024 19:01:42 -0600 Subject: [PATCH 34/45] switched to future from event --- src/digger/digger/digger_node.py | 41 +++++++++++++++++++------------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 5e16dc51..ca95d40c 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -13,7 +13,7 @@ from rovr_interfaces.srv import SetPower, SetPosition from rovr_interfaces.msg import LimitSwitches from std_srvs.srv import Trigger -from threading import Event, Thread +from concurrent.futures import Future, ThreadPoolExecutor class DiggerNode(Node): def __init__(self): @@ -63,8 +63,10 @@ def __init__(self): # Limit Switch States self.top_limit_pressed = False self.bottom_limit_pressed = False - self.top_limit_event = Event() - self.bottom_limit_event = Event() + self.top_limit_event = Future() + self.bottom_limit_event = Future() + self.limitExecutor = ThreadPoolExecutor(max_workers=1) + #One worker bc it should only be zeroing or lifting at once # Maximum value of the lift motor encoder (bottom of the lift system) IN DEGREES @@ -97,8 +99,8 @@ def toggle(self, digger_belt_power: float) -> None: def stop_lift(self) -> None: """This method stops the lift.""" self.lift_running = False - self.top_limit_event.set() - self.bottom_limit_event.set() + self.top_limit_event.set_result(0) + self.bottom_limit_event.set_result(0) self.cli_motor_set.call_async( MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=0.0) ) @@ -171,24 +173,29 @@ def lift_set_power_callback(self, request, response): def zero_lift_callback(self, request, response): """This service request zeros the lift system.""" - self.top_limit_event.clear() - self.zero_lift() - self.top_limit_event.wait() + self.bottom_limit_event.set_result(1) + #Free poolExecutor Resources + self.limitExecutor.submit(self.zero_lift()) + #self.zero_lift() + self.top_limit_event.result() #while not self.top_limit_pressed and self.running: #pass response.success = True - self.top_limit_event.clear() + self.top_limit_event = Future() return response def lower_lift_callback(self, request, response): """This service request reverse-zeros the lift system, putting it at the lowest point""" - self.bottom_limit_event.clear() - self.lower_lift() - self.bottom_limit_event.wait() + self.top_limit_event.set_result(1) + #Free poolExecutor Resources^ + self.limitExecutor.submit(self.lower_lift()) + #self.bottom_limit_event.clear() + #self.lower_lift() + self.bottom_limit_event.result() #while not self.bottom_limit_pressed and self.running: #pass response.success = True - self.bottom_limit_event.clear() + self.bottom_limit_event = Future() return response # No more timer callback because setPos works @@ -203,14 +210,14 @@ def limit_switch_callback(self, limit_switches_msg): self.top_limit_pressed = limit_switches_msg.top_limit_switch self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch if self.top_limit_pressed: # If the top limit switch is pressed - self.top_limit_event.set() - self.bottom_limit_event.clear() + self.top_limit_event.set_result(0) + #self.bottom_limit_event = Future() self.lift_encoder_offset = self.current_position_degrees self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) elif self.bottom_limit_pressed: # If the bottom limit switch is pressed - self.bottom_limit_event.set() - self.top_limit_event.clear() + #self.top_limit_event = Future() + self.bottom_limit_event.set_result(0) self.lift_encoder_offset = self.current_position_degrees - self.MAX_ENCODER_DEGREES self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) From 67c4146944e4a656e89d48771f0d11ac37e41a6f Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 21 Nov 2024 19:35:56 -0600 Subject: [PATCH 35/45] trying smthn new with MultiThreadedExecutor --- src/digger/digger/digger_node.py | 47 ++++++++++---------------------- 1 file changed, 15 insertions(+), 32 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index ca95d40c..9f82531b 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -6,14 +6,14 @@ # Import the ROS 2 Python module import rclpy from rclpy.node import Node - +from rclpy.executor import MultiThreadedExecutor # Import custom ROS 2 interfaces from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet from rovr_interfaces.srv import SetPower, SetPosition from rovr_interfaces.msg import LimitSwitches from std_srvs.srv import Trigger -from concurrent.futures import Future, ThreadPoolExecutor + class DiggerNode(Node): def __init__(self): @@ -63,10 +63,6 @@ def __init__(self): # Limit Switch States self.top_limit_pressed = False self.bottom_limit_pressed = False - self.top_limit_event = Future() - self.bottom_limit_event = Future() - self.limitExecutor = ThreadPoolExecutor(max_workers=1) - #One worker bc it should only be zeroing or lifting at once # Maximum value of the lift motor encoder (bottom of the lift system) IN DEGREES @@ -99,8 +95,6 @@ def toggle(self, digger_belt_power: float) -> None: def stop_lift(self) -> None: """This method stops the lift.""" self.lift_running = False - self.top_limit_event.set_result(0) - self.bottom_limit_event.set_result(0) self.cli_motor_set.call_async( MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=0.0) ) @@ -173,29 +167,18 @@ def lift_set_power_callback(self, request, response): def zero_lift_callback(self, request, response): """This service request zeros the lift system.""" - self.bottom_limit_event.set_result(1) - #Free poolExecutor Resources - self.limitExecutor.submit(self.zero_lift()) - #self.zero_lift() - self.top_limit_event.result() - #while not self.top_limit_pressed and self.running: - #pass + self.zero_lift() + while not self.top_limit_pressed and self.running: + rclpy.spin_once(self) response.success = True - self.top_limit_event = Future() return response def lower_lift_callback(self, request, response): """This service request reverse-zeros the lift system, putting it at the lowest point""" - self.top_limit_event.set_result(1) - #Free poolExecutor Resources^ - self.limitExecutor.submit(self.lower_lift()) - #self.bottom_limit_event.clear() - #self.lower_lift() - self.bottom_limit_event.result() - #while not self.bottom_limit_pressed and self.running: - #pass + self.lower_lift() + while not self.bottom_limit_pressed and self.running: + rclpy.spin_once(self) response.success = True - self.bottom_limit_event = Future() return response # No more timer callback because setPos works @@ -210,14 +193,10 @@ def limit_switch_callback(self, limit_switches_msg): self.top_limit_pressed = limit_switches_msg.top_limit_switch self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch if self.top_limit_pressed: # If the top limit switch is pressed - self.top_limit_event.set_result(0) - #self.bottom_limit_event = Future() self.lift_encoder_offset = self.current_position_degrees self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) elif self.bottom_limit_pressed: # If the bottom limit switch is pressed - #self.top_limit_event = Future() - self.bottom_limit_event.set_result(0) self.lift_encoder_offset = self.current_position_degrees - self.MAX_ENCODER_DEGREES self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) @@ -229,11 +208,15 @@ def main(args=None): node = DiggerNode() node.get_logger().info("Initializing the Digger subsystem!") - rclpy.spin(node) - + executor = MultiThreadedExecutor() + executor.add_node(node) + #rclpy.spin(node) + executor.spin() + + executor.shutdown() node.destroy_node() rclpy.shutdown() - + # This code does NOT run if this file is imported as a module if __name__ == "__main__": From 413388a540eaa736eed1efad817942eaa380c63d Mon Sep 17 00:00:00 2001 From: cparece1 Date: Thu, 21 Nov 2024 19:53:53 -0600 Subject: [PATCH 36/45] trying to fix import --- src/digger/digger/digger_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 9f82531b..26c030cf 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -6,7 +6,7 @@ # Import the ROS 2 Python module import rclpy from rclpy.node import Node -from rclpy.executor import MultiThreadedExecutor +from rclpy.executors import MultiThreadedExecutor # Import custom ROS 2 interfaces from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet From d1d9f34c8c10a960c14156c2a44e8403566afc57 Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Thu, 21 Nov 2024 20:07:34 -0600 Subject: [PATCH 37/45] fixed running to be lift running --- src/digger/digger/digger_node.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 26c030cf..6f9dcf25 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -168,15 +168,16 @@ def lift_set_power_callback(self, request, response): def zero_lift_callback(self, request, response): """This service request zeros the lift system.""" self.zero_lift() - while not self.top_limit_pressed and self.running: + while not self.top_limit_pressed and self.lift_running: rclpy.spin_once(self) + self.get_logger().info("T2" + str(self.lift_running)) response.success = True return response def lower_lift_callback(self, request, response): """This service request reverse-zeros the lift system, putting it at the lowest point""" self.lower_lift() - while not self.bottom_limit_pressed and self.running: + while not self.bottom_limit_pressed and self.lift_running: rclpy.spin_once(self) response.success = True return response From 570e9ce3fbe18ff22fe2eb91dbd8048b821c496a Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 21 Nov 2024 20:12:18 -0600 Subject: [PATCH 38/45] Digger node with futures to control limit switch behavior --- src/digger/digger/digger_node.py | 84 +++++++++++--------------------- 1 file changed, 28 insertions(+), 56 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 6f9dcf25..134b8bcd 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -4,6 +4,7 @@ # Last Updated: October 2024 # Import the ROS 2 Python module +from warnings import deprecated import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor @@ -13,7 +14,7 @@ from rovr_interfaces.srv import SetPower, SetPosition from rovr_interfaces.msg import LimitSwitches from std_srvs.srv import Trigger - +from rclpy.task import Future class DiggerNode(Node): def __init__(self): @@ -63,6 +64,8 @@ def __init__(self): # Limit Switch States self.top_limit_pressed = False self.bottom_limit_pressed = False + self.top_limit_event = Future() + self.bottom_limit_event = Future() # Maximum value of the lift motor encoder (bottom of the lift system) IN DEGREES @@ -99,28 +102,6 @@ def stop_lift(self) -> None: MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=0.0) ) - def lift_set_power(self, power: float) -> None: - """This method sets power to the lift system.""" - self.lift_running = True - if power > 0 and self.top_limit_pressed: - self.get_logger().warn("WARNING: Top limit switch pressed!") - self.stop_lift() # Stop the lift system - return - if power < 0 and self.bottom_limit_pressed: - self.get_logger().warn("WARNING: Bottom limit switch pressed!") - self.stop_lift() # Stop the lift system - return - self.cli_motor_set.call_async( - MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=power) - ) - - def zero_lift(self) -> None: - """This method zeros the lift system by slowly raising it until the top limit switch is pressed.""" - self.lift_set_power(0.05) - - def lower_lift(self) -> None: - self.lift_set_power(-0.05) - # Define service callback methods here def set_power_callback(self, request, response): """This service request sets power to the digger belt.""" @@ -141,8 +122,9 @@ def toggle_callback(self, request, response): return response async def set_position_callback(self, request, response): - """This service request sets the position of the lift.""" - """This method sets the position (in degrees) of the digger.""" + """This service request sets the position of the lift. + TODO: Make sure MotorCommandSet.Request(type="position", + is cancellable otherwise this will fail""" await self.cli_motor_set.call_async( MotorCommandSet.Request( type="position", @@ -158,49 +140,39 @@ def stop_lift_callback(self, request, response): self.stop_lift() response.success = True return response - + + @deprecated def lift_set_power_callback(self, request, response): """This service request sets power to the digger belt.""" - self.lift_set_power(request.power) - response.success = True return response - def zero_lift_callback(self, request, response): + async def zero_lift_callback(self, request, response): """This service request zeros the lift system.""" - self.zero_lift() - while not self.top_limit_pressed and self.lift_running: - rclpy.spin_once(self) - self.get_logger().info("T2" + str(self.lift_running)) - response.success = True + self.lift_set_power(0.05) + self.top_limit_event = Future() + self.top_limit_event.add_done_callback(self.stop_lift) + await self.bottom_limit_event return response - def lower_lift_callback(self, request, response): + async def lower_lift_callback(self, request, response): """This service request reverse-zeros the lift system, putting it at the lowest point""" - self.lower_lift() - while not self.bottom_limit_pressed and self.lift_running: - rclpy.spin_once(self) - response.success = True - return response - - # No more timer callback because setPos works + if self.bottom_limit_pressed: + return response + self.lift_set_power(-0.05) + self.bottom_limit_event = Future() + self.bottom_limit_event.add_done_callback(self.stop_lift) + await self.bottom_limit_event + return self.bottom_limit_event.result() # Define subscriber callback methods here def limit_switch_callback(self, limit_switches_msg): """This subscriber callback method is called whenever a message is received on the limitSwitches topic.""" - if not self.top_limit_pressed and limit_switches_msg.top_limit_switch: - self.stop_lift() # Stop the lift system - if not self.bottom_limit_pressed and limit_switches_msg.bottom_limit_switch: - self.stop_lift() # Stop the lift system - self.top_limit_pressed = limit_switches_msg.top_limit_switch - self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch - if self.top_limit_pressed: # If the top limit switch is pressed - self.lift_encoder_offset = self.current_position_degrees - self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) - self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) - elif self.bottom_limit_pressed: # If the bottom limit switch is pressed - self.lift_encoder_offset = self.current_position_degrees - self.MAX_ENCODER_DEGREES - self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) - self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) + if self.top_limit_pressed and not self.top_limit_event.done(): + self.top_limit_pressed = limit_switches_msg.top_limit_switch + self.top_limit_event.set_result(True) + if self.bottom_limit_pressed and not self.top_limit_event.done(): + self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch + self.top_limit_event.set_result(True) def main(args=None): From 114bb1968622a2c0595e4964d03f783dc5e30116 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Tue, 26 Nov 2024 19:15:01 -0600 Subject: [PATCH 39/45] fixed zero callback --- src/digger/digger/digger_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 134b8bcd..f46a7c9e 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -151,7 +151,7 @@ async def zero_lift_callback(self, request, response): self.lift_set_power(0.05) self.top_limit_event = Future() self.top_limit_event.add_done_callback(self.stop_lift) - await self.bottom_limit_event + await self.top_limit_event return response async def lower_lift_callback(self, request, response): From 610e5072f1bbc89117c30df54075d4cb02de7471 Mon Sep 17 00:00:00 2001 From: cparece1 Date: Tue, 26 Nov 2024 19:21:06 -0600 Subject: [PATCH 40/45] Cleaned up --- src/digger/digger/digger_node.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index f46a7c9e..90c37dfc 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -98,6 +98,8 @@ def toggle(self, digger_belt_power: float) -> None: def stop_lift(self) -> None: """This method stops the lift.""" self.lift_running = False + self.top_limit_event.set_result(False) + self.bottom_limit_event.set_result(False) self.cli_motor_set.call_async( MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=0.0) ) @@ -172,7 +174,7 @@ def limit_switch_callback(self, limit_switches_msg): self.top_limit_event.set_result(True) if self.bottom_limit_pressed and not self.top_limit_event.done(): self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch - self.top_limit_event.set_result(True) + self.bottom_limit_event.set_result(True) def main(args=None): From 6949ae213e908d150feebcecb51157baf857dba3 Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Sun, 12 Jan 2025 17:08:51 -0600 Subject: [PATCH 41/45] small bug fix --- src/rovr_control/rovr_control/auto_offload_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rovr_control/rovr_control/auto_offload_server.py b/src/rovr_control/rovr_control/auto_offload_server.py index 3eb1a852..bad8c683 100644 --- a/src/rovr_control/rovr_control/auto_offload_server.py +++ b/src/rovr_control/rovr_control/auto_offload_server.py @@ -1,5 +1,4 @@ import rclpy -from rclpy.node import Node from rclpy.action import ActionServer from rclpy.action.server import CancelResponse, ServerGoalHandle @@ -8,6 +7,7 @@ from rovr_interfaces.srv import Drive from std_srvs.srv import Trigger +from rovr_control.node_util import AsyncNode class AutoOffloadServer(AsyncNode): def __init__(self): From 55048dc617c043967063640d7e47c769ac2eb152 Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Sun, 12 Jan 2025 17:12:53 -0600 Subject: [PATCH 42/45] small change --- src/dumper/dumper/dumper_node.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index e22719cb..a4fb74ac 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -101,9 +101,9 @@ async def toggle_callback(self, request, response): response.success = True return response - def extend_dumper(self) -> None: + async def extend_dumper(self) -> None: self.top_limit_event.clear() - self.set_power(self.DUMPER_POWER) + await self.set_power(self.DUMPER_POWER) self.top_limit_event.wait() #while not self.top_limit_pressed and self.running: #pass @@ -116,9 +116,9 @@ async def extend_callback(self, request, response): response.success = True return response - def retract_dumper(self) -> None: + async def retract_dumper(self) -> None: self.bottom_limit_event.clear() - self.set_power(-self.DUMPER_POWER) + await self.set_power(-self.DUMPER_POWER) self.bottom_limit_event.wait() #while not self.bottom_limit_pressed and self.running: #pass From bc68b1b4bc399667169be4e9b9c6da0ef34edb5e Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Sun, 12 Jan 2025 19:53:27 -0600 Subject: [PATCH 43/45] Update auto_offload_server.py --- .../rovr_control/auto_offload_server.py | 20 +++++-------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/src/rovr_control/rovr_control/auto_offload_server.py b/src/rovr_control/rovr_control/auto_offload_server.py index e6e212a0..ea5b20bf 100644 --- a/src/rovr_control/rovr_control/auto_offload_server.py +++ b/src/rovr_control/rovr_control/auto_offload_server.py @@ -1,5 +1,4 @@ import rclpy - from rclpy.action import ActionServer from rclpy.action.server import CancelResponse, ServerGoalHandle @@ -9,6 +8,7 @@ from rovr_control.node_util import AsyncNode + class AutoOffloadServer(AsyncNode): def __init__(self): super().__init__("auto_offload_server") @@ -23,12 +23,8 @@ def __init__(self): self.cli_drivetrain_drive = self.create_client(Drive, "drivetrain/drive") self.cli_drivetrain_stop = self.create_client(Trigger, "drivetrain/stop") + self.cli_dumper_dump = self.create_client(Trigger, "dumper/dump") self.cli_dumper_stop = self.create_client(Trigger, "dumper/stop") - self.cli_dumper_extend = self.create_client(Trigger, "dumper/extendDumper") - self.cli_dumper_retract = self.create_client(Trigger, "dumper/retractDumper") - - self.declare_parameter("dump_time", 5) - self.dumpTime = self.get_parameter("dump_time").value async def execute_callback(self, goal_handle: ServerGoalHandle): """This method lays out the procedure for autonomously offloading!""" @@ -44,12 +40,8 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): self.get_logger().error("Drivetrain stop service not available") goal_handle.abort() return result - if not self.cli_dumper_extend.wait_for_service(timeout_sec=1.0): - self.get_logger().error("Dumper extend service not available") - goal_handle.abort() - return result - if not self.cli_dumper_retract.wait_for_service(timeout_sec=1.0): - self.get_logger().error("Dumper retract service not available") + if not self.cli_dumper_dump.wait_for_service(timeout_sec=1.0): + self.get_logger().error("Dumper dump service not available") goal_handle.abort() return result if not self.cli_dumper_stop.wait_for_service(timeout_sec=1.0): @@ -67,9 +59,7 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): # Dump the material self.get_logger().info("Auto Dumping") - await self.cli_dumper_extend.call_async(Trigger.Request()) # Extend - await self.async_sleep(self.dumpTime) - await self.cli_dumper_retract.call_async(Trigger.Request()) # Retract + await self.cli_dumper_dump.call_async(Trigger.Request()) self.get_logger().info("Autonomous Offload Procedure Complete!") goal_handle.succeed() From 723011122f7a629203cd7d3ee41915a2411a0272 Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Sun, 12 Jan 2025 19:56:35 -0600 Subject: [PATCH 44/45] Added missing newlines back --- src/dumper/dumper/dumper_node.py | 2 +- src/rovr_control/rovr_control/node_util.py | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index f4839530..da23760c 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -175,4 +175,4 @@ def main(args=None): # This code does NOT run if this file is imported as a module if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/src/rovr_control/rovr_control/node_util.py b/src/rovr_control/rovr_control/node_util.py index 207ae0b0..c85bba62 100644 --- a/src/rovr_control/rovr_control/node_util.py +++ b/src/rovr_control/rovr_control/node_util.py @@ -2,8 +2,6 @@ from rclpy.node import Node from rclpy.task import Future -from std_msgs.msg import Bool - class AsyncNode(Node): def __init__(self, name: str): @@ -30,4 +28,4 @@ def cancel_callback(self, cancel_request: ServerGoalHandle): self.timer.cancel() self.timer.destroy() return CancelResponse.ACCEPT - return CancelResponse.REJECT \ No newline at end of file + return CancelResponse.REJECT From 8bf6bded08492243faeae42d3b1d07e6c15111b2 Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Sun, 12 Jan 2025 20:42:03 -0600 Subject: [PATCH 45/45] Updated DiggerNode to use the same MultiThreadedExecutor and MutuallyExclusiveCallbackGroup system as the DumperNode --- src/digger/digger/digger_node.py | 122 ++++++++++++++++++++----------- src/dumper/dumper/dumper_node.py | 4 +- 2 files changed, 82 insertions(+), 44 deletions(-) diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py index 53e77617..d3648f08 100644 --- a/src/digger/digger/digger_node.py +++ b/src/digger/digger/digger_node.py @@ -1,39 +1,61 @@ # This ROS 2 node contains code for the digger subsystem of the robot. # Original Author: Anthony Brogni in Fall 2023 -# Maintainer: Charlie Parece -# Last Updated: October 2024 +# Maintainer: Anthony Brogni +# Last Updated: January 2025 + +import time # Import the ROS 2 Python module -from warnings import deprecated import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup + +# Import ROS 2 formatted message types # Import custom ROS 2 interfaces from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet from rovr_interfaces.srv import SetPower, SetPosition from rovr_interfaces.msg import LimitSwitches from std_srvs.srv import Trigger -from rclpy.task import Future + class DiggerNode(Node): def __init__(self): """Initialize the ROS 2 digger node.""" super().__init__("digger") + self.service_cb_group = MutuallyExclusiveCallbackGroup() + # Define service clients here self.cli_motor_set = self.create_client(MotorCommandSet, "motor/set") self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get") # Define services (methods callable from the outside) here - self.srv_toggle = self.create_service(SetPower, "digger/toggle", self.toggle_callback) - self.srv_stop = self.create_service(Trigger, "digger/stop", self.stop_callback) - self.srv_setPower = self.create_service(SetPower, "digger/setPower", self.set_power_callback) - self.srv_setPosition = self.create_service(SetPosition, "lift/setPosition", self.set_position_callback) - self.srv_lift_stop = self.create_service(Trigger, "lift/stop", self.stop_lift_callback) - self.srv_lift_set_power = self.create_service(SetPower, "lift/setPower", self.lift_set_power_callback) - self.srv_zero_lift = self.create_service(Trigger, "lift/zero", self.zero_lift_callback) - self.srv_lower_lift = self.create_service(Trigger, "lift/lower", self.lower_lift_callback) + self.srv_toggle = self.create_service( + SetPower, "digger/toggle", self.toggle_callback, callback_group=self.service_cb_group + ) + self.srv_stop = self.create_service( + Trigger, "digger/stop", self.stop_callback, callback_group=self.service_cb_group + ) + self.srv_setPower = self.create_service( + SetPower, "digger/setPower", self.set_power_callback, callback_group=self.service_cb_group + ) + self.srv_setPosition = self.create_service( + SetPosition, "lift/setPosition", self.set_position_callback, callback_group=self.service_cb_group + ) + self.srv_lift_stop = self.create_service( + Trigger, "lift/stop", self.stop_lift_callback, callback_group=self.service_cb_group + ) + self.srv_lift_set_power = self.create_service( + SetPower, "lift/setPower", self.lift_set_power_callback, callback_group=self.service_cb_group + ) + self.srv_zero_lift = self.create_service( + Trigger, "lift/zero", self.zero_lift_callback, callback_group=self.service_cb_group + ) + self.srv_bottom_lift = self.create_service( + Trigger, "lift/bottom", self.bottom_lift_callback, callback_group=self.service_cb_group + ) # Define publishers here @@ -64,9 +86,6 @@ def __init__(self): # Limit Switch States self.top_limit_pressed = False self.bottom_limit_pressed = False - self.top_limit_event = Future() - self.bottom_limit_event = Future() - # Maximum value of the lift motor encoder (bottom of the lift system) IN DEGREES self.MAX_ENCODER_DEGREES = ( @@ -98,12 +117,39 @@ def toggle(self, digger_belt_power: float) -> None: def stop_lift(self) -> None: """This method stops the lift.""" self.lift_running = False - self.top_limit_event.set_result(False) - self.bottom_limit_event.set_result(False) self.cli_motor_set.call_async( MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=0.0) ) + def lift_set_power(self, power: float) -> None: + """This method sets power to the lift system.""" + self.lift_running = True + if power > 0 and self.top_limit_pressed: + self.get_logger().warn("WARNING: Top limit switch pressed!") + self.stop_lift() # Stop the lift system + return + if power < 0 and self.bottom_limit_pressed: + self.get_logger().warn("WARNING: Bottom limit switch pressed!") + self.stop_lift() # Stop the lift system + return + self.cli_motor_set.call_async( + MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=power) + ) + + def zero_lift(self) -> None: + """This method zeros the lift system by slowly raising it until the top limit switch is pressed.""" + while not self.top_limit_pressed: + self.lift_set_power(0.05) + time.sleep(0.1) # We don't want to spam loop iterations too fast + self.stop_lift() + + def bottom_lift(self) -> None: + """This method bottoms out the lift system by slowly lowering it until the bottom limit switch is pressed.""" + while not self.bottom_limit_pressed: + self.lift_set_power(-0.05) + time.sleep(0.1) # We don't want to spam loop iterations too fast + self.stop_lift() + # Define service callback methods here def set_power_callback(self, request, response): """This service request sets power to the digger belt.""" @@ -125,7 +171,7 @@ def toggle_callback(self, request, response): async def set_position_callback(self, request, response): """This service request sets the position of the lift. - TODO: Make sure MotorCommandSet.Request(type="position", + TODO: Make sure MotorCommandSet.Request(type="position", is cancellable otherwise this will fail""" await self.cli_motor_set.call_async( MotorCommandSet.Request( @@ -142,37 +188,28 @@ def stop_lift_callback(self, request, response): self.stop_lift() response.success = True return response - - @deprecated + def lift_set_power_callback(self, request, response): """This service request sets power to the digger belt.""" + self.lift_set_power(request.power) + response.success = True return response - async def zero_lift_callback(self, request, response): + def zero_lift_callback(self, request, response): """This service request zeros the lift system.""" - self.lift_set_power(0.05) - self.top_limit_event = Future() - self.top_limit_event.add_done_callback(self.stop_lift) - await self.top_limit_event + self.zero_lift() + response.success = True return response - async def lower_lift_callback(self, request, response): - """This service request reverse-zeros the lift system, putting it at the lowest point""" - if self.bottom_limit_pressed: - return response - self.lift_set_power(-0.05) - self.bottom_limit_event = Future() - self.bottom_limit_event.add_done_callback(self.stop_lift) - await self.bottom_limit_event - return self.bottom_limit_event.result() + def bottom_lift_callback(self, request, response): + """This service request bottoms out the lift system.""" + self.bottom_lift() + response.success = True + return response # Define subscriber callback methods here def limit_switch_callback(self, limit_switches_msg): """This subscriber callback method is called whenever a message is received on the limitSwitches topic.""" - if self.top_limit_pressed and not self.top_limit_event.done(): - self.top_limit_event.set_result(True) - if self.bottom_limit_pressed and not self.top_limit_event.done(): - self.bottom_limit_event.set_result(True) if not self.top_limit_pressed and limit_switches_msg.digger_top_limit_switch: self.stop_lift() # Stop the lift system if not self.bottom_limit_pressed and limit_switches_msg.digger_bottom_limit_switch: @@ -194,16 +231,15 @@ def main(args=None): rclpy.init(args=args) node = DiggerNode() - node.get_logger().info("Initializing the Digger subsystem!") executor = MultiThreadedExecutor() executor.add_node(node) - #rclpy.spin(node) + + node.get_logger().info("Initializing the Digger subsystem!") executor.spin() - - executor.shutdown() + node.destroy_node() rclpy.shutdown() - + # This code does NOT run if this file is imported as a module if __name__ == "__main__": diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index da23760c..f3d06a90 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -114,6 +114,7 @@ def toggle_callback(self, request, response): def extend_dumper(self) -> None: while not self.top_limit_pressed: self.set_power(self.DUMPER_POWER) + time.sleep(0.1) # We don't want to spam loop iterations too fast self.stop() def extend_callback(self, request, response): @@ -125,6 +126,7 @@ def extend_callback(self, request, response): def retract_dumper(self) -> None: while not self.bottom_limit_pressed: self.set_power(-self.DUMPER_POWER) + time.sleep(0.1) # We don't want to spam loop iterations too fast self.stop() def retract_callback(self, request, response): @@ -138,7 +140,7 @@ def dump(self) -> None: self.extend_dumper() self.get_logger().info("Dumper extended!") # wait for 5 seconds before retracting the dumper - time.sleep(5) # Allows for task to be canceled + time.sleep(5) # retract the dumper self.retract_dumper() self.get_logger().info("Dumper retracted!")