From b23642325635bdbc1d4fb7a696e7c99ce2f6415b Mon Sep 17 00:00:00 2001 From: Colten G Date: Tue, 24 Mar 2026 20:07:09 -0400 Subject: [PATCH 1/2] Updated jetson code to utilize Henrys ESP32 code, needs review --- mrobosub_hal/mrobosub_hal/esp32_thruster.py | 144 ++++++++++++++++++ mrobosub_hal/mrobosub_hal/pololu.py | 131 ---------------- .../mrobosub_hal/thruster_controller.py | 14 +- mrobosub_msgs/msg/PololuCommands.msg | 2 - mrobosub_msgs/msg/ThrusterCommands.msg | 2 + 5 files changed, 153 insertions(+), 140 deletions(-) create mode 100644 mrobosub_hal/mrobosub_hal/esp32_thruster.py delete mode 100644 mrobosub_hal/mrobosub_hal/pololu.py delete mode 100644 mrobosub_msgs/msg/PololuCommands.msg create mode 100644 mrobosub_msgs/msg/ThrusterCommands.msg diff --git a/mrobosub_hal/mrobosub_hal/esp32_thruster.py b/mrobosub_hal/mrobosub_hal/esp32_thruster.py new file mode 100644 index 00000000..f4df696b --- /dev/null +++ b/mrobosub_hal/mrobosub_hal/esp32_thruster.py @@ -0,0 +1,144 @@ +# in-progress +import rclpy +from rcl_interfaces.msg import ParameterDescriptor + +from mrobosub_lib import Node +from serial import Serial +from serial.serialutil import SerialException +from mrobosub_msgs.msg import ThrusterCommands +from std_srvs.srv import SetBool +from typing import Optional + + +NUM_PINS = 12 +FREQUENCY = 50 + + +class ESP32_Thruster(Node): + """ + Provides requires /esp32/thruster_commands topic + """ + + def __init__(self): + super().__init__("esp32_thruster") + self.get_logger().info("Launched esp32_thruster node") + self.port = "/dev/thruster_esp" #IMPORTANT: DETERMINE ACTUAL PORT + self.thruster_outputs = [0] * NUM_PINS + self.serial: Serial | None = None + self.connect() + + self.esp32_thruster_sub = self.create_subscription( + ThrusterCommands, "/esp32/thruster_commands", self.eps32_thruster_commands_callback, 1 + ) + self.emergency_stop_service = self.create_service( + SetBool, "emergency_stop_motors", self.handle_emergency_stop + ) + self.timer = self.create_timer(1.0/FREQUENCY, self.loop) + + + def connect(self) -> bool: + try: + self.serial = Serial(self.port, timeout=0.5, write_timeout=0.5) + enable_all() + except SerialException as e: + self.get_logger().info(f"Could not connect to esp32_thruster: {e}") + return False + return True + + def write(self, data: str) -> bool: + if self.serial is None: + success = self.connect() + if not success or self.serial is None: + return False + try: + self.serial.write(data) + return True + except SerialException as e: + self.get_logger().info(f"write error: {e}") + self.serial.close() + self.connect() + return False + + + # percent_raw should be in [-1, 1] + # percent_val should be in [-100, 100] + def convert_percent(self, percent_raw: float) -> Optional[int]: + if percent_raw < -1 or percent_raw > 1: + self.get_logger().info( + f"ESP32 [ERROR]: Raw input value {percent_raw} out of range (should be in [-1, 1])" + ) + return None + return int(percent_raw * 100) + + # in case of invalid percent or pin number parameters, does not send any updated signal to the pin controller + def send_power(self, pin: int, percent_raw: float) -> int: + percent_val = self.convert_percent(percent_raw) + if percent_val is None: + return -1 + + if pin < 0 or pin >= NUM_PINS: + self.get_logger().info( + f"ERROR: pin number {pin} out of range (should be in [0, {NUM_PINS-1}])" + ) + return -1 + + msg = "POWER:" + str(pin) + "," + str(percent_val) + + self.write(msg) + + return 0 + + + def send_enable_disable(self, pin: int, enable: bool) -> int: + if(enable == true): + msg = "ENABLE:TRUE" + else: + msg = "ENABLE:FALSE" + + if pin < 0 or pin >= NUM_PINS: + self.get_logger().info( + f"ERROR: pin number {pin} out of range (should be in [0, {NUM_PINS-1}])" + ) + return -1 + + self.write(msg) + + def enable_all(self): + for i in range(NUM_PINS): + send_enable_disable(i, true) + + + def send_estop(self): + msg = "ESTOP:" + self.write(msg) + + + def handle_emergency_stop( + self, req: SetBool.Request, res: SetBool.Response + ) -> SetBool.Response: + if req.data: + send_estop + res.success = True + return res + + def eps32_thruster_commands_callback(self, msg: ThrusterCommands): + for i in range(NUM_PINS): + if msg.valid[i]: + self.thruster_outputs[i] = msg.pins[i] + + def loop(self): + for i in range(NUM_PINS): + self.send_power(i, self.thruster_outputs[i]) + + +def main(): + rclpy.init() + node = ESP32_Thruster() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + +if __name__ == "__main__": + main() diff --git a/mrobosub_hal/mrobosub_hal/pololu.py b/mrobosub_hal/mrobosub_hal/pololu.py deleted file mode 100644 index 9d85a9a5..00000000 --- a/mrobosub_hal/mrobosub_hal/pololu.py +++ /dev/null @@ -1,131 +0,0 @@ -# in-progress -import rclpy -from rcl_interfaces.msg import ParameterDescriptor - -from mrobosub_lib import Node -from serial import Serial -from serial.serialutil import SerialException -from mrobosub_msgs.msg import PololuCommands -from std_srvs.srv import SetBool -from typing import Optional - - -NUM_PINS = 12 -FREQUENCY = 50 - - -class Pololu(Node): - """ - Provides requires /pololu_commands topic - """ - - def __init__(self): - super().__init__("pololu") - self.get_logger().info("Launched pololu node") - self.port = "/dev/serial/by-id/usb-Pololu_Corporation_Pololu_Mini_Maestro_12-Channel_USB_Servo_Controller_00467345-if00" - self.pololu_outputs = [0] * NUM_PINS - self.serial: Serial | None = None - self.connect() - self.get_errors() # clear errors at the start - - self.pololu_sub = self.create_subscription( - PololuCommands, "/pololu_commands", self.pololu_commands_callback, 1 - ) - self.timer = self.create_timer(1.0/FREQUENCY, self.loop) - - def connect(self) -> bool: - try: - self.serial = Serial(self.port, timeout=0.5, write_timeout=0.5) - except SerialException as e: - self.get_logger().info(f"Could not connect to mini maestro: {e}") - return False - return True - - def write(self, data: bytearray) -> bool: - if self.serial is None: - success = self.connect() - if not success or self.serial is None: - return False - try: - self.serial.write(data) - return True - except SerialException as e: - self.get_logger().info(f"write error: {e}") - self.serial.close() - self.connect() - return False - - def read(self, len: int) -> Optional[bytes]: - if self.serial is None: - success = self.connect() - if not success or self.serial is None: - return None - try: - return self.serial.read(len) - except SerialException as e: - self.get_logger().info(f"read error: {e}") - self.serial.close() - self.connect() - return None - - # pwm_raw should be in [-1, 1] - # pwm_val should be in [4000, 8000] - def convert_pwm_signal(self, pwm_raw: float) -> Optional[int]: - if pwm_raw < -1 or pwm_raw > 1: - self.get_logger().info( - f"Pololu [ERROR]: PWM value {pwm_raw} out of range (should be in [-1, 1])" - ) - return None - return int((pwm_raw * 1600) + 6000) - - # in case of invalid PWM or pin number parameters, does not send any updated signal to the pin controller - def send_signal(self, pin: int, pwm_raw: float) -> int: - pwm_val = self.convert_pwm_signal(pwm_raw) - if pwm_val is None: - return -1 - - if pin < 0 or pin >= NUM_PINS: - self.get_logger().info( - f"ERROR: pin number {pin} out of range (should be in [0, {NUM_PINS-1}])" - ) - return -1 - - LSBs = pwm_val % (2**7) - MSBs = int(pwm_val / (2**7)) - - self.get_errors() - self.write(bytearray([0xAA, 0x0C, 0x04, pin, LSBs, MSBs])) - - return 0 - - def pololu_commands_callback(self, msg: PololuCommands): - for i in range(NUM_PINS): - if msg.valid[i]: - self.pololu_outputs[i] = msg.pins[i] - - def get_errors(self): - self.write(bytearray([0xAA, 0x0C, 0x21])) - error = self.read(2) - if error is None: - return - error_code = int.from_bytes(error, "little") - if error_code != 0: - self.get_logger().info(f"Pololu: error code = {error_code}") - # eg: error_code 16 means 00010000 which is the 5th error bit set - - def loop(self): - for i in range(NUM_PINS): - self.send_signal(i, self.pololu_outputs[i]) - - -def main(): - rclpy.init() - node = Pololu() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - - -if __name__ == "__main__": - main() diff --git a/mrobosub_hal/mrobosub_hal/thruster_controller.py b/mrobosub_hal/mrobosub_hal/thruster_controller.py index 0d847105..a97804c9 100755 --- a/mrobosub_hal/mrobosub_hal/thruster_controller.py +++ b/mrobosub_hal/mrobosub_hal/thruster_controller.py @@ -5,7 +5,7 @@ from serial import Serial from serial.serialutil import SerialException from mrobosub_msgs.msg import MotorState -from mrobosub_msgs.msg import PololuCommands +from mrobosub_msgs.msg import ThrusterCommands from std_srvs.srv import SetBool from typing import Optional @@ -33,8 +33,8 @@ def __init__(self): self.motor_sub = self.create_subscription( MotorState, "/motor_output", self.motor_callback, 1 ) - self.pololu_pub = self.create_publisher( - PololuCommands, "/pololu_commands", qos_profile=1 + self.esp32_thruster_pub = self.create_publisher( + ThrusterCommands, "/esp32/thruster_commands", qos_profile=1 ) params = [Param('thruster_reverse', rclpy.Parameter.Type.BOOL_ARRAY, "List of booleans indicating whether each thruster is reversed"), @@ -57,8 +57,8 @@ def handle_emergency_stop( # in case of invalid PWM or motor number parameters, does not send any updated signal to the motor controller def publish_motor_outputs(self, msg: MotorState) -> int: - message_valid = [False]*12 - message_output = [0.0]*12 + message_valid = [False]*NUM_MOTORS + message_output = [0.0]*NUM_MOTORS if self.emergency_stop: for motor in range(NUM_MOTORS): @@ -79,12 +79,12 @@ def publish_motor_outputs(self, msg: MotorState) -> int: message_output[motor_pin] = msg.motors[motor] - msg = PololuCommands() + msg = ThrusterCommands() message_output = np.array(message_output, dtype=np.float32) msg.pins = message_output msg.valid = message_valid - self.pololu_pub.publish(msg) + self.esp32_thruster_pub.publish(msg) return 0 diff --git a/mrobosub_msgs/msg/PololuCommands.msg b/mrobosub_msgs/msg/PololuCommands.msg deleted file mode 100644 index 1c3f5393..00000000 --- a/mrobosub_msgs/msg/PololuCommands.msg +++ /dev/null @@ -1,2 +0,0 @@ -float32[12] pins -bool[12] valid \ No newline at end of file diff --git a/mrobosub_msgs/msg/ThrusterCommands.msg b/mrobosub_msgs/msg/ThrusterCommands.msg new file mode 100644 index 00000000..30fcd758 --- /dev/null +++ b/mrobosub_msgs/msg/ThrusterCommands.msg @@ -0,0 +1,2 @@ +float32[8] pins +bool[8] valid \ No newline at end of file From ab0bdc1bb2cefa3ed1190d374d119ffcc3fab16a Mon Sep 17 00:00:00 2001 From: Colten G Date: Tue, 24 Mar 2026 22:07:11 -0400 Subject: [PATCH 2/2] Hopefully fixes working tree and doesn't break it more --- mrobosub_hal/mrobosub_hal/esp32_thruster.py | 9 +++++---- mrobosub_hal/setup.py | 2 +- mrobosub_msgs/CMakeLists.txt | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/mrobosub_hal/mrobosub_hal/esp32_thruster.py b/mrobosub_hal/mrobosub_hal/esp32_thruster.py index f4df696b..ccddab70 100644 --- a/mrobosub_hal/mrobosub_hal/esp32_thruster.py +++ b/mrobosub_hal/mrobosub_hal/esp32_thruster.py @@ -74,19 +74,19 @@ def convert_percent(self, percent_raw: float) -> Optional[int]: def send_power(self, pin: int, percent_raw: float) -> int: percent_val = self.convert_percent(percent_raw) if percent_val is None: - return -1 + return False if pin < 0 or pin >= NUM_PINS: self.get_logger().info( f"ERROR: pin number {pin} out of range (should be in [0, {NUM_PINS-1}])" ) - return -1 + return False msg = "POWER:" + str(pin) + "," + str(percent_val) self.write(msg) - return 0 + return True def send_enable_disable(self, pin: int, enable: bool) -> int: @@ -99,9 +99,10 @@ def send_enable_disable(self, pin: int, enable: bool) -> int: self.get_logger().info( f"ERROR: pin number {pin} out of range (should be in [0, {NUM_PINS-1}])" ) - return -1 + return False self.write(msg) + return True def enable_all(self): for i in range(NUM_PINS): diff --git a/mrobosub_hal/setup.py b/mrobosub_hal/setup.py index 831e5abf..78b072d8 100644 --- a/mrobosub_hal/setup.py +++ b/mrobosub_hal/setup.py @@ -28,7 +28,7 @@ "dvl_publisher = mrobosub_hal.dvl_publisher:main", "botcam = mrobosub_hal.botcam:main", "zed = mrobosub_hal.zed:main", - "pololu = mrobosub_hal.pololu:main", + "esp32_thruster = mrobosub_hal.esp32_controller:main", "arduino = mrobosub_hal.arduino:main" ], }, diff --git a/mrobosub_msgs/CMakeLists.txt b/mrobosub_msgs/CMakeLists.txt index 6460e9a0..c788f208 100644 --- a/mrobosub_msgs/CMakeLists.txt +++ b/mrobosub_msgs/CMakeLists.txt @@ -19,7 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ImuINS.msg" "msg/ImuPIMU.msg" "msg/MotorState.msg" - "msg/PololuCommands.msg" + "msg/ThrusterCommands.msg" "msg/Pose.msg" "msg/Twist.msg" "srv/BinCamPos.srv"