From 5aa8419b754877e1360007c30ea60b375c1b93fc Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 19 Sep 2026 13:55:52 -0400 Subject: [PATCH 1/2] feat: add Teensy communication node --- .github/workflows/ci.yml | 2 +- setup/README.md | 2 +- src/sailing/package.xml | 17 ++ src/sailing/resource/sailing | 0 src/sailing/sailing/__init__.py | 1 + src/sailing/sailing/constants.py | 48 +++++ src/sailing/sailing/teensy/__init__.py | 1 + src/sailing/sailing/teensy/serial_port.py | 139 +++++++++++++++ src/sailing/sailing/teensy/teensy_node.py | 202 +++++++++++++++++++++ src/sailing/setup.cfg | 4 + src/sailing/setup.py | 23 +++ src/sailing/test/conftest.py | 52 ++++++ src/sailing/test/test_serial_port.py | 112 ++++++++++++ src/sailing/test/test_teensy_node.py | 207 ++++++++++++++++++++++ 14 files changed, 808 insertions(+), 2 deletions(-) create mode 100644 src/sailing/package.xml create mode 100644 src/sailing/resource/sailing create mode 100644 src/sailing/sailing/__init__.py create mode 100644 src/sailing/sailing/constants.py create mode 100644 src/sailing/sailing/teensy/__init__.py create mode 100644 src/sailing/sailing/teensy/serial_port.py create mode 100644 src/sailing/sailing/teensy/teensy_node.py create mode 100644 src/sailing/setup.cfg create mode 100644 src/sailing/setup.py create mode 100644 src/sailing/test/conftest.py create mode 100644 src/sailing/test/test_serial_port.py create mode 100644 src/sailing/test/test_teensy_node.py diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f5bcfe6..485e4da 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -45,7 +45,7 @@ jobs: boat-ci bash -c ' set -e test_status=0 - colcon test --return-code-on-test-failure || test_status=$? + colcon test --python-testing pytest --return-code-on-test-failure || test_status=$? colcon test-result --verbose exit "$test_status" ' diff --git a/setup/README.md b/setup/README.md index ab1b050..c2ca3a2 100644 --- a/setup/README.md +++ b/setup/README.md @@ -124,7 +124,7 @@ this shell; ROS 2 does not need to be installed on your host. In the container shell opened above, run the package tests and inspect the results: ```bash -colcon test --return-code-on-test-failure +colcon test --python-testing pytest --return-code-on-test-failure colcon test-result --verbose ``` diff --git a/src/sailing/package.xml b/src/sailing/package.xml new file mode 100644 index 0000000..914a384 --- /dev/null +++ b/src/sailing/package.xml @@ -0,0 +1,17 @@ + + + sailing + 0.0.1 + Sailing control and hardware interfaces. + ericcai32 + Apache License 2.0 + + ament_python + rclpy + std_msgs + python3-pytest + + + ament_python + + diff --git a/src/sailing/resource/sailing b/src/sailing/resource/sailing new file mode 100644 index 0000000..e69de29 diff --git a/src/sailing/sailing/__init__.py b/src/sailing/sailing/__init__.py new file mode 100644 index 0000000..c6255b9 --- /dev/null +++ b/src/sailing/sailing/__init__.py @@ -0,0 +1 @@ +"""Sailing control and hardware interfaces.""" diff --git a/src/sailing/sailing/constants.py b/src/sailing/sailing/constants.py new file mode 100644 index 0000000..9f4058b --- /dev/null +++ b/src/sailing/sailing/constants.py @@ -0,0 +1,48 @@ +"""Boat limits, Teensy node defaults, and serial protocol constants.""" + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class _Physical: + """Actuator limits in degrees and jib-side conventions.""" + + RUDDER_MIN_ANGLE: int = -45 + RUDDER_MAX_ANGLE: int = 45 + MAINSAIL_MIN_ANGLE: int = 0 + MAINSAIL_MAX_ANGLE: int = 90 + JIB_MIN_ANGLE: int = 0 + JIB_MAX_ANGLE: int = 90 + JIB_SIDE_PORT: int = 0 + JIB_SIDE_STB: int = 1 + + +@dataclass(frozen=True) +class _Serial: + """Serial protocol values; TX/RX are from the Teensy's perspective.""" + + TX_START_FLAG: int = 0xFF + TX_END_FLAG: int = 0xEE + TX_PERIOD_MS: int = 500 + TX_PACKET_LEN: int = 7 + RX_START_FLAG: int = 0xFF + RX_END_FLAG: int = 0xEE + RX_PERIOD_MS: int = 500 + BAUD_RATE: int = 9600 + + +@dataclass(frozen=True) +class _Teensy: + """Default settings and initial actuator positions for the Teensy node.""" + + DEFAULT_PORT: str = "/dev/ttyACM0" + DEFAULT_TELEMETRY_POLL_PERIOD_SECONDS: float = 0.5 + INITIAL_MAINSAIL_ANGLE: int = 0 + INITIAL_RUDDER_ANGLE: int = 0 + INITIAL_JIB_ANGLE: int = 0 + WARNING_THROTTLE_SECONDS: float = 5.0 + + +PHYSICAL = _Physical() +SERIAL = _Serial() +TEENSY = _Teensy() diff --git a/src/sailing/sailing/teensy/__init__.py b/src/sailing/sailing/teensy/__init__.py new file mode 100644 index 0000000..f73fbd7 --- /dev/null +++ b/src/sailing/sailing/teensy/__init__.py @@ -0,0 +1 @@ +"""ROS 2 bridge for Teensy commands and telemetry.""" diff --git a/src/sailing/sailing/teensy/serial_port.py b/src/sailing/sailing/teensy/serial_port.py new file mode 100644 index 0000000..fece6e2 --- /dev/null +++ b/src/sailing/sailing/teensy/serial_port.py @@ -0,0 +1,139 @@ +import serial + +from sailing.constants import PHYSICAL, SERIAL + + +class SerialPort: + """Handle serial commands and telemetry for the Teensy.""" + + def __init__(self, port): + """Open the serial connection.""" + self.port = port + self.buffer = [] + self.packet_started = False + self.serial = serial.Serial(self.port, baudrate=SERIAL.BAUD_RATE) + + def send_command(self, mainsail_angle, rudder_angle, jib_angle, jib_side_flag): + """ + Send a properly formatted command packet to the Teensy. + Return 0 on success or 1 if encoding or writing fails. + :param mainsail_angle: new mainsail angle to set (integer). Should be in range [0, 90]. + :param rudder_angle: new rudder angle to set (integer). Should be in range [-45, 45]. + :param jib_angle: new jib angle to set (integer). Should be in range [10, 80]. + :param jib_side_flag: side to set the jib on (0 = port, 1 = starboard). + """ + try: + # Check bounds. For the sails, this is just defensive. + mainsail_angle = max(min(mainsail_angle, 127), -128) + rudder_angle = rudder_angle - PHYSICAL.RUDDER_MIN_ANGLE + jib_angle = max(min(jib_angle, 127), -128) + + # Convert control values to 8-bit integers (bytes). The else check is just defensive. + mainsail_byte = ( + mainsail_angle & 0xFF + if mainsail_angle >= 0 + else (mainsail_angle + 256) & 0xFF + ) + rudder_byte = ( + rudder_angle & 0xFF + if rudder_angle >= 0 + else (rudder_angle + 256) & 0xFF + ) + jib_angle_byte = ( + jib_angle & 0xFF if jib_angle >= 0 else (jib_angle + 256) & 0xFF + ) + jib_side_byte = jib_side_flag & 0xFF + + # Command payload format between start/end flags: + # [mainsail_angle, rudder_angle, jib_angle, jib_side_flag] + command_packet = bytearray( + [ + SERIAL.RX_START_FLAG, + mainsail_byte, + rudder_byte, + jib_angle_byte, + jib_side_byte, + SERIAL.RX_END_FLAG, + ] + ) + + # Send the packet over serial. + self.serial.write(command_packet) + return 0 + except Exception: + return 1 + + def read_telemetry(self, data): + """Read a telemetry packet into data; return 0 on success, otherwise 1. + + Firmware must send exactly SERIAL.TX_PACKET_LEN payload bytes between + SERIAL.TX_START_FLAG (0xFF) and SERIAL.TX_END_FLAG (0xEE). Neither flag value may + occur anywhere in the payload, including wind bytes and counters. + Payload order: [wind_hi, wind_lo, mainsail_angle, rudder_angle, + jib_angle, jib_side_flag, dropped_packets]. + + Partial packets persist between calls. After a successful read, + queued serial input is discarded, matching the original driver. + """ + # Check for waiting serial data. + while self.serial.in_waiting > 0: + incoming_byte = self.serial.read() + + # If we see a packet start byte: set flags, clear buffer. + if incoming_byte == SERIAL.TX_START_FLAG.to_bytes(1, "big"): + self.packet_started = True + self.buffer = [] + # If we see a packet end byte and the buffer is full, process the buffer and store into ``data``. + elif ( + self.packet_started + and incoming_byte == SERIAL.TX_END_FLAG.to_bytes(1, "big") + and len(self.buffer) == SERIAL.TX_PACKET_LEN + ): + self.packet_started = False + ( + data["wind_angle"], + data["mainsail_angle"], + data["rudder_angle"], + data["jib_angle"], + data["jib_side_flag"], + data["dropped_packets"], + ) = self._parse_packet(self.buffer) + self.serial.reset_input_buffer() # Clear buffer. + return 0 + # If we have previously seen a packet start byte, add data to our buffer. + elif self.packet_started: + self.buffer.append(int.from_bytes(incoming_byte, "big")) + return 1 + + def _parse_packet(self, packet): + """Decode the seven-byte telemetry payload, excluding framing flags.""" + wind_angle = (packet[0] << 8) | packet[1] + + # uint8_t to int8_t conversion for negative values. + mainsail_angle = packet[2] - 256 if packet[2] >= 128 else packet[2] + mainsail_angle = ( + PHYSICAL.MAINSAIL_MAX_ANGLE - mainsail_angle + ) # invert for mechanical swap + rudder_angle = packet[3] - 256 if packet[3] >= 128 else packet[3] + rudder_angle -= -PHYSICAL.RUDDER_MIN_ANGLE # undo the command offset + jib_angle = packet[4] - 256 if packet[4] >= 128 else packet[4] + jib_angle = ( + PHYSICAL.JIB_MAX_ANGLE + PHYSICAL.JIB_MIN_ANGLE - jib_angle + ) # invert for mechanical swap + + jib_side_flag = packet[5] + dropped_packets = packet[6] + + return ( + wind_angle, + mainsail_angle, + rudder_angle, + jib_angle, + jib_side_flag, + dropped_packets, + ) + + def close(self): + """Close the serial connection.""" + if hasattr(self, "serial"): + self.serial.close() diff --git a/src/sailing/sailing/teensy/teensy_node.py b/src/sailing/sailing/teensy/teensy_node.py new file mode 100644 index 0000000..d921084 --- /dev/null +++ b/src/sailing/sailing/teensy/teensy_node.py @@ -0,0 +1,202 @@ +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import Int32, UInt8 + +from sailing.constants import PHYSICAL, TEENSY + +from . import serial_port + + +class Teensy(Node): + """ + ROS 2 Node that interfaces with a Teensy microcontroller to control and + monitor the boat's hardware (mainsail, rudder, and jib). It communicates + with the Teensy, reads telemetry (wind, angles, dropped packets), and sends + control commands from ROS subscriptions. + + Command topics (namespace-relative): ``sail`` (Int32), ``rudder_angle`` + (Int32), ``jib_angle`` (Int32), ``jib_side_flag`` (UInt8: 0 = port side, + 1 = starboard side). + """ + + def __init__(self): + super().__init__("teensy") + + # Declare parameters. + self.declare_parameter("teensy_port", TEENSY.DEFAULT_PORT) + self.declare_parameter( + "rx_period", TEENSY.DEFAULT_TELEMETRY_POLL_PERIOD_SECONDS + ) + + # Read parameters. + self.telemetry_poll_period = float(self.get_parameter("rx_period").value) + self.teensy_port = self.get_parameter("teensy_port").value + + if self.telemetry_poll_period <= 0: + raise ValueError("Telemetry polling period must be positive.") + + # Initialize the hardware driver. + try: + self.serial_port = serial_port.SerialPort(self.teensy_port) + except Exception as exc: + self.get_logger().error( + f"Serial connection failed: port={self.teensy_port} reason={exc}" + ) + raise + self.get_logger().info( + f"Serial connection opened: port={self.teensy_port} " + f"poll_period={self.telemetry_poll_period}s" + ) + + # Initialize desired actuator positions. + self.desired_mainsail_angle = TEENSY.INITIAL_MAINSAIL_ANGLE + self.desired_rudder_angle = TEENSY.INITIAL_RUDDER_ANGLE + self.desired_jib_angle = TEENSY.INITIAL_JIB_ANGLE + self.desired_jib_side_flag = PHYSICAL.JIB_SIDE_PORT + + # Create telemetry publishers. + self.actual_mainsail_angle_pub = self.create_publisher( + Int32, "actual_sail_angle", 10 + ) + self.actual_rudder_angle_pub = self.create_publisher( + Int32, "actual_rudder_angle", 10 + ) + self.actual_jib_angle_pub = self.create_publisher(Int32, "actual_jib_angle", 10) + self.actual_jib_side_flag_pub = self.create_publisher( + UInt8, "actual_jib_side_flag", 10 + ) + self.dropped_packets_pub = self.create_publisher(Int32, "dropped_packets", 10) + + # Poll telemetry periodically. + self.timer = self.create_timer(self.telemetry_poll_period, self.check_telemetry) + + # Subscribe to actuator commands. + self.create_subscription(Int32, "sail", self.mainsail_angle_callback, 10) + self.create_subscription(Int32, "rudder_angle", self.rudder_angle_callback, 10) + self.create_subscription(Int32, "jib_angle", self.jib_angle_callback, 10) + self.create_subscription( + UInt8, "jib_side_flag", self.jib_side_flag_callback, 10 + ) + + def mainsail_angle_callback(self, msg): + """Update the mainsail goal and send the full command.""" + self.desired_mainsail_angle = msg.data + self._send_command_to_teensy() + + def rudder_angle_callback(self, msg): + """Update the rudder goal and send the full command.""" + self.desired_rudder_angle = msg.data + self._send_command_to_teensy() + + def jib_angle_callback(self, msg): + """Update the jib goal and send the full command.""" + self.desired_jib_angle = msg.data + self._send_command_to_teensy() + + def jib_side_flag_callback(self, msg): + """Update the jib side and send the full command.""" + self.desired_jib_side_flag = int(msg.data) + self._send_command_to_teensy() + + def check_telemetry(self): + """Read and publish telemetry from the Teensy.""" + data = {} + try: + received = self.serial_port.read_telemetry(data) == 0 + except OSError as exc: + self.get_logger().warning( + f"Telemetry read failed: reason={exc}", + throttle_duration_sec=TEENSY.WARNING_THROTTLE_SECONDS, + ) + return + + if received: + mainsail_angle_msg = Int32() + mainsail_angle_msg.data = data["mainsail_angle"] + self.actual_mainsail_angle_pub.publish(mainsail_angle_msg) + + rudder_angle_msg = Int32() + rudder_angle_msg.data = data["rudder_angle"] + self.actual_rudder_angle_pub.publish(rudder_angle_msg) + + jib_angle_msg = Int32() + jib_angle_msg.data = data["jib_angle"] + self.actual_jib_angle_pub.publish(jib_angle_msg) + + jib_side_flag_msg = UInt8() + jib_side_flag_msg.data = data["jib_side_flag"] + self.actual_jib_side_flag_pub.publish(jib_side_flag_msg) + + dropped_packets_msg = Int32() + dropped_packets_msg.data = data["dropped_packets"] + self.dropped_packets_pub.publish(dropped_packets_msg) + + side = ( + "port" + if data["jib_side_flag"] == PHYSICAL.JIB_SIDE_PORT + else "starboard" + ) + self.get_logger().debug( + f"RX mainsail={data['mainsail_angle']} rudder={data['rudder_angle']} " + f"jib={data['jib_angle']} jib_side={side} " + f"dropped_packets={data['dropped_packets']}" + ) + else: + self.get_logger().debug("No telemetry received.") + + def _send_command_to_teensy(self): + """Send the latest mainsail, rudder, and jib goals in one packet.""" + side = ( + "port" + if self.desired_jib_side_flag == PHYSICAL.JIB_SIDE_PORT + else "starboard" + ) + command = ( + f"mainsail={self.desired_mainsail_angle} rudder={self.desired_rudder_angle} " + f"jib={self.desired_jib_angle} jib_side={side}" + ) + try: + result = self.serial_port.send_command( + self.desired_mainsail_angle, + self.desired_rudder_angle, + self.desired_jib_angle, + self.desired_jib_side_flag, + ) + except (OSError, ValueError) as exc: + reason = str(exc) + else: + if result == 0: + self.get_logger().debug(f"TX {command}") + return + reason = f"driver returned status {result}" + self.get_logger().warning( + f"Command failed: {command} reason={reason}", + throttle_duration_sec=TEENSY.WARNING_THROTTLE_SECONDS, + ) + + def destroy_node(self): + """Close the serial connection and release ROS resources.""" + try: + self.serial_port.close() + finally: + result = super().destroy_node() + return result + + +def main(args=None): + """Run the Teensy node and release resources on exit.""" + rclpy.init(args=args) + teensy_node = Teensy() + + try: + rclpy.spin(teensy_node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + teensy_node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/sailing/setup.cfg b/src/sailing/setup.cfg new file mode 100644 index 0000000..8d26e60 --- /dev/null +++ b/src/sailing/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sailing +[install] +install_scripts=$base/lib/sailing diff --git a/src/sailing/setup.py b/src/sailing/setup.py new file mode 100644 index 0000000..5973629 --- /dev/null +++ b/src/sailing/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup + +package_name = "sailing" + +setup( + name=package_name, + version="0.0.1", + packages=find_packages(include=[package_name, f"{package_name}.*"]), + data_files=[ + ( + "share/ament_index/resource_index/packages", + [f"resource/{package_name}"], + ), + (f"share/{package_name}", ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="ericcai32", + maintainer_email="ericcai32@gmail.com", + description="Sailing control and hardware interfaces.", + license="Apache License 2.0", + entry_points={"console_scripts": ["teensy_node = sailing.teensy.teensy_node:main"]}, +) diff --git a/src/sailing/test/conftest.py b/src/sailing/test/conftest.py new file mode 100644 index 0000000..6740a40 --- /dev/null +++ b/src/sailing/test/conftest.py @@ -0,0 +1,52 @@ +"""Fake serial connection shared by driver tests.""" + +from unittest.mock import Mock + +import pytest +from sailing.teensy import serial_port + + +class FakeSerial: + def __init__(self): + self.incoming = bytearray() + self.written = bytearray() + self.is_open = True + self.write_error = None + self.read_error = None + + @property + def in_waiting(self): + if self.read_error: + raise self.read_error + return len(self.incoming) + + def read(self, size=1): + data = bytes(self.incoming[:size]) + del self.incoming[:size] + return data + + def write(self, data): + if self.write_error: + raise self.write_error + self.written.extend(data) + return len(data) + + def reset_input_buffer(self): + self.incoming.clear() + + def close(self): + self.is_open = False + + +@pytest.fixture +def transport(monkeypatch): + transport = FakeSerial() + monkeypatch.setattr(serial_port.serial, "Serial", Mock(return_value=transport)) + return transport + + +@pytest.fixture +def driver(transport): + driver = serial_port.SerialPort("/dev/test-teensy") + yield driver + driver.close() diff --git a/src/sailing/test/test_serial_port.py b/src/sailing/test/test_serial_port.py new file mode 100644 index 0000000..3dd5dfa --- /dev/null +++ b/src/sailing/test/test_serial_port.py @@ -0,0 +1,112 @@ +"""Regression tests for the migrated serial driver's legacy behavior. + +Cover framing, sail telemetry inversions, rudder wire center 45, command +encoding, error handling, and cleanup using a fake serial connection. +Telemetry payloads must exclude start and end flag values. These tests +preserve the migration's behavior; they do not verify hardware calibration. +""" + +import pytest +from sailing.constants import PHYSICAL +from sailing.teensy import serial_port + + +def frame(wind=300, mainsail=20, rudder=45, jib=30, side=1, dropped=7): + return bytes( + [0xFF, wind >> 8, wind & 0xFF, mainsail, rudder, jib, side, dropped, 0xEE] + ) + + +def test_open_uses_configured_port_and_baudrate(transport): + serial_port.SerialPort("/dev/test-teensy") + args, kwargs = serial_port.serial.Serial.call_args + assert (args[0] if args else kwargs["port"]) == "/dev/test-teensy" + assert kwargs["baudrate"] == 9600 + + +def test_open_failure_is_visible(transport): + serial_port.serial.Serial.side_effect = OSError("disconnected") + with pytest.raises(OSError, match="disconnected"): + serial_port.SerialPort("/dev/test-teensy") + + +def test_empty_read_leaves_data_unchanged(driver): + data = {"previous": 123} + assert driver.read_telemetry(data) == 1 + assert data == {"previous": 123} + + +def test_decode_telemetry(driver, transport): + transport.incoming.extend(frame()) + data = {} + assert driver.read_telemetry(data) == 0 + # Preserve the original mechanical-swap inversions for both sails. + assert data == dict( + wind_angle=300, + mainsail_angle=70, + rudder_angle=0, + jib_angle=60, + jib_side_flag=1, + dropped_packets=7, + ) + + +@pytest.mark.parametrize("split", range(1, 9)) +def test_fragmented_frame(driver, transport, split): + packet = frame() + transport.incoming.extend(packet[:split]) + data = {"previous": 123} + assert driver.read_telemetry(data) == 1 + assert data == {"previous": 123} + transport.incoming.extend(packet[split:]) + assert driver.read_telemetry(data) == 0 + assert data["wind_angle"] == 300 + + +@pytest.mark.parametrize( + "garbage", [b"noise", frame()[:-1] + b"\x00", b"\xff" + b"\x01" * 64] +) +def test_resynchronize_after_noise_or_bad_frame(driver, transport, garbage): + transport.incoming.extend(garbage + frame()) + data = {} + for _ in range(3): + if driver.read_telemetry(data) == 0: + break + assert data.get("wind_angle") == 300 + + +@pytest.mark.parametrize("position", ["minimum", "neutral", "maximum"]) +def test_command_bytes(driver, transport, position): + rudder = { + "minimum": PHYSICAL.RUDDER_MIN_ANGLE, + "neutral": 0, + "maximum": PHYSICAL.RUDDER_MAX_ANGLE, + }[position] + # The migrated protocol uses 45 as the rudder wire center. + wire = rudder + 45 + side = PHYSICAL.JIB_SIDE_STB + assert driver.send_command(20, rudder, 30, side) == 0 + assert transport.written == bytes([255, 20, wire, 30, side, 238]) + + +@pytest.mark.parametrize("side", [-1, 2]) +def test_side_is_encoded_as_unsigned_byte(driver, transport, side): + assert driver.send_command(20, 0, 30, side) == 0 + assert transport.written == bytes([255, 20, 45, 30, side & 255, 238]) + + +def test_read_failure_is_visible(driver, transport): + transport.read_error = OSError("unplugged") + with pytest.raises(OSError, match="unplugged"): + driver.read_telemetry({}) + + +def test_write_failure_returns_failure_status(driver, transport): + transport.write_error = OSError("unplugged") + assert driver.send_command(20, 0, 30, 0) == 1 + assert not transport.written + + +def test_close_closes_connection(driver, transport): + driver.close() + assert not transport.is_open diff --git a/src/sailing/test/test_teensy_node.py b/src/sailing/test/test_teensy_node.py new file mode 100644 index 0000000..be7c8c3 --- /dev/null +++ b/src/sailing/test/test_teensy_node.py @@ -0,0 +1,207 @@ +"""Real ROS tests with only the hardware driver faked.""" + +import time +import uuid +from dataclasses import replace +from unittest.mock import Mock, call + +import pytest +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from sailing.teensy import teensy_node +from std_msgs.msg import Int32, UInt8 + + +@pytest.fixture +def ros(): + rclpy.init(args=["--ros-args", "-r", "__ns:=/test_" + uuid.uuid4().hex]) + yield + rclpy.shutdown() + + +@pytest.fixture +def ros_node(ros, monkeypatch): + hardware = Mock( + send_command=Mock(return_value=0), read_telemetry=Mock(return_value=1) + ) + monkeypatch.setattr( + teensy_node.serial_port, "SerialPort", Mock(return_value=hardware) + ) + node = teensy_node.Teensy() + peer = Node("test_peer") + executor = SingleThreadedExecutor() + executor.add_node(node) + executor.add_node(peer) + yield node, peer, hardware, executor + executor.shutdown() + peer.destroy_node() + # Cleanup also works after the explicit close-failure test. + hardware.close.side_effect = None + node.destroy_node() + + +def spin_until(executor, predicate): + deadline = time.monotonic() + 10 + while not predicate() and time.monotonic() < deadline: + executor.spin_once(timeout_sec=0.05) + assert predicate(), "Timed out waiting for ROS" + + +def observe_telemetry(node, peer, executor): + received = {} + topics = { + "actual_sail_angle": Int32, + "actual_rudder_angle": Int32, + "actual_jib_angle": Int32, + "actual_jib_side_flag": UInt8, + "dropped_packets": Int32, + } + subscriptions = [ + peer.create_subscription( + message_type, + topic, + lambda msg, topic=topic: received.setdefault(topic, []).append(msg.data), + 10, + ) + for topic, message_type in topics.items() + ] + spin_until( + executor, + lambda: all( + peer.count_publishers(subscription.topic_name) == 1 + and node.count_subscribers(subscription.topic_name) == 1 + for subscription in subscriptions + ), + ) + return received + + +def test_command_topics_send_full_latest_command(ros_node): + node, peer, hardware, executor = ros_node + node.timer.cancel() + commands = [ + ("sail", Int32, 20), + ("rudder_angle", Int32, -10), + ("jib_angle", Int32, 30), + ("jib_side_flag", UInt8, 1), + ] + for count, (topic, message_type, value) in enumerate(commands, start=1): + publisher = peer.create_publisher(message_type, topic, 10) + spin_until(executor, lambda: publisher.get_subscription_count() == 1) + publisher.publish(message_type(data=value)) + spin_until(executor, lambda: hardware.send_command.call_count >= count) + assert hardware.send_command.call_args_list == [ + call(20, 0, 0, 0), + call(20, -10, 0, 0), + call(20, -10, 30, 0), + call(20, -10, 30, 1), + ] + + +def test_timer_publishes_complete_telemetry(ros_node): + node, peer, hardware, executor = ros_node + received = observe_telemetry(node, peer, executor) + sample = dict( + wind_angle=300, + mainsail_angle=20, + rudder_angle=-10, + jib_angle=30, + jib_side_flag=1, + dropped_packets=7, + ) + + def read(data): + data.update(sample) + return 0 + + hardware.read_telemetry.side_effect = read + expected = { + "actual_sail_angle": 20, + "actual_rudder_angle": -10, + "actual_jib_angle": 30, + "actual_jib_side_flag": 1, + "dropped_packets": 7, + } + spin_until(executor, lambda: set(received) == set(expected)) + assert {topic: values[-1] for topic, values in received.items()} == expected + + +@pytest.mark.parametrize("error", [None, OSError("unplugged")]) +def test_no_publication_on_missing_or_failed_read(ros_node, monkeypatch, error): + node, peer, hardware, executor = ros_node + node.timer.cancel() + received = observe_telemetry(node, peer, executor) + logger = Mock(wraps=node.get_logger()) + monkeypatch.setattr(node, "get_logger", lambda: logger) + hardware.read_telemetry.side_effect = error + node.check_telemetry() + deadline = time.monotonic() + 0.2 + spin_until(executor, lambda: time.monotonic() >= deadline) + assert not received + if error: + logger.warning.assert_called_once() + assert logger.warning.call_args.kwargs["throttle_duration_sec"] > 0 + else: + logger.warning.assert_not_called() + + +@pytest.mark.parametrize( + "error", [None, OSError("unplugged"), ValueError("invalid angle")] +) +def test_command_failure_is_reported(ros_node, monkeypatch, error): + node, _, hardware, _ = ros_node + logger = Mock(wraps=node.get_logger()) + monkeypatch.setattr(node, "get_logger", lambda: logger) + hardware.send_command.return_value = 1 + hardware.send_command.side_effect = error + node.rudder_angle_callback(Int32(data=10)) + logger.warning.assert_called_once() + assert "Command failed" in logger.warning.call_args.args[0] + if error: + assert str(error) in logger.warning.call_args.args[0] + + +@pytest.mark.parametrize("period", [0, -0.1]) +def test_invalid_poll_period_does_not_open_port(ros, monkeypatch, period): + monkeypatch.setattr( + teensy_node, + "TEENSY", + replace(teensy_node.TEENSY, DEFAULT_TELEMETRY_POLL_PERIOD_SECONDS=period), + ) + factory = Mock() + monkeypatch.setattr(teensy_node.serial_port, "SerialPort", factory) + node = teensy_node.Teensy.__new__(teensy_node.Teensy) + try: + with pytest.raises(ValueError): + node.__init__() + factory.assert_not_called() + finally: + Node.destroy_node(node) + + +def test_serial_open_failure_propagates(ros, monkeypatch): + monkeypatch.setattr( + teensy_node.serial_port, "SerialPort", Mock(side_effect=OSError("missing port")) + ) + node = teensy_node.Teensy.__new__(teensy_node.Teensy) + try: + with pytest.raises(OSError, match="missing port"): + node.__init__() + finally: + Node.destroy_node(node) + + +@pytest.mark.parametrize("error", [None, OSError("close failed")]) +def test_shutdown_releases_ros_even_if_serial_close_fails(ros_node, error): + node, _, hardware, _ = ros_node + hardware.close.side_effect = error + if error: + with pytest.raises(OSError, match="close failed"): + node.destroy_node() + else: + node.destroy_node() + hardware.close.assert_called_once() + assert not list(node.publishers) + assert not list(node.subscriptions) + assert not list(node.timers) From a17a3fd90d385f67d3f75ae22a4b70191d88ac2e Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 19 Sep 2026 13:55:52 -0400 Subject: [PATCH 2/2] feat: add Teensy communication node --- .github/workflows/ci.yml | 2 +- setup/README.md | 2 +- src/sailing/package.xml | 17 ++ src/sailing/resource/sailing | 0 src/sailing/sailing/__init__.py | 1 + src/sailing/sailing/constants.py | 48 +++++ src/sailing/sailing/teensy/__init__.py | 1 + src/sailing/sailing/teensy/serial_port.py | 139 +++++++++++++++ src/sailing/sailing/teensy/teensy_node.py | 204 +++++++++++++++++++++ src/sailing/setup.cfg | 4 + src/sailing/setup.py | 23 +++ src/sailing/test/conftest.py | 52 ++++++ src/sailing/test/test_serial_port.py | 112 ++++++++++++ src/sailing/test/test_teensy_node.py | 207 ++++++++++++++++++++++ 14 files changed, 810 insertions(+), 2 deletions(-) create mode 100644 src/sailing/package.xml create mode 100644 src/sailing/resource/sailing create mode 100644 src/sailing/sailing/__init__.py create mode 100644 src/sailing/sailing/constants.py create mode 100644 src/sailing/sailing/teensy/__init__.py create mode 100644 src/sailing/sailing/teensy/serial_port.py create mode 100644 src/sailing/sailing/teensy/teensy_node.py create mode 100644 src/sailing/setup.cfg create mode 100644 src/sailing/setup.py create mode 100644 src/sailing/test/conftest.py create mode 100644 src/sailing/test/test_serial_port.py create mode 100644 src/sailing/test/test_teensy_node.py diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f5bcfe6..485e4da 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -45,7 +45,7 @@ jobs: boat-ci bash -c ' set -e test_status=0 - colcon test --return-code-on-test-failure || test_status=$? + colcon test --python-testing pytest --return-code-on-test-failure || test_status=$? colcon test-result --verbose exit "$test_status" ' diff --git a/setup/README.md b/setup/README.md index ab1b050..c2ca3a2 100644 --- a/setup/README.md +++ b/setup/README.md @@ -124,7 +124,7 @@ this shell; ROS 2 does not need to be installed on your host. In the container shell opened above, run the package tests and inspect the results: ```bash -colcon test --return-code-on-test-failure +colcon test --python-testing pytest --return-code-on-test-failure colcon test-result --verbose ``` diff --git a/src/sailing/package.xml b/src/sailing/package.xml new file mode 100644 index 0000000..914a384 --- /dev/null +++ b/src/sailing/package.xml @@ -0,0 +1,17 @@ + + + sailing + 0.0.1 + Sailing control and hardware interfaces. + ericcai32 + Apache License 2.0 + + ament_python + rclpy + std_msgs + python3-pytest + + + ament_python + + diff --git a/src/sailing/resource/sailing b/src/sailing/resource/sailing new file mode 100644 index 0000000..e69de29 diff --git a/src/sailing/sailing/__init__.py b/src/sailing/sailing/__init__.py new file mode 100644 index 0000000..c6255b9 --- /dev/null +++ b/src/sailing/sailing/__init__.py @@ -0,0 +1 @@ +"""Sailing control and hardware interfaces.""" diff --git a/src/sailing/sailing/constants.py b/src/sailing/sailing/constants.py new file mode 100644 index 0000000..9f4058b --- /dev/null +++ b/src/sailing/sailing/constants.py @@ -0,0 +1,48 @@ +"""Boat limits, Teensy node defaults, and serial protocol constants.""" + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class _Physical: + """Actuator limits in degrees and jib-side conventions.""" + + RUDDER_MIN_ANGLE: int = -45 + RUDDER_MAX_ANGLE: int = 45 + MAINSAIL_MIN_ANGLE: int = 0 + MAINSAIL_MAX_ANGLE: int = 90 + JIB_MIN_ANGLE: int = 0 + JIB_MAX_ANGLE: int = 90 + JIB_SIDE_PORT: int = 0 + JIB_SIDE_STB: int = 1 + + +@dataclass(frozen=True) +class _Serial: + """Serial protocol values; TX/RX are from the Teensy's perspective.""" + + TX_START_FLAG: int = 0xFF + TX_END_FLAG: int = 0xEE + TX_PERIOD_MS: int = 500 + TX_PACKET_LEN: int = 7 + RX_START_FLAG: int = 0xFF + RX_END_FLAG: int = 0xEE + RX_PERIOD_MS: int = 500 + BAUD_RATE: int = 9600 + + +@dataclass(frozen=True) +class _Teensy: + """Default settings and initial actuator positions for the Teensy node.""" + + DEFAULT_PORT: str = "/dev/ttyACM0" + DEFAULT_TELEMETRY_POLL_PERIOD_SECONDS: float = 0.5 + INITIAL_MAINSAIL_ANGLE: int = 0 + INITIAL_RUDDER_ANGLE: int = 0 + INITIAL_JIB_ANGLE: int = 0 + WARNING_THROTTLE_SECONDS: float = 5.0 + + +PHYSICAL = _Physical() +SERIAL = _Serial() +TEENSY = _Teensy() diff --git a/src/sailing/sailing/teensy/__init__.py b/src/sailing/sailing/teensy/__init__.py new file mode 100644 index 0000000..f73fbd7 --- /dev/null +++ b/src/sailing/sailing/teensy/__init__.py @@ -0,0 +1 @@ +"""ROS 2 bridge for Teensy commands and telemetry.""" diff --git a/src/sailing/sailing/teensy/serial_port.py b/src/sailing/sailing/teensy/serial_port.py new file mode 100644 index 0000000..fece6e2 --- /dev/null +++ b/src/sailing/sailing/teensy/serial_port.py @@ -0,0 +1,139 @@ +import serial + +from sailing.constants import PHYSICAL, SERIAL + + +class SerialPort: + """Handle serial commands and telemetry for the Teensy.""" + + def __init__(self, port): + """Open the serial connection.""" + self.port = port + self.buffer = [] + self.packet_started = False + self.serial = serial.Serial(self.port, baudrate=SERIAL.BAUD_RATE) + + def send_command(self, mainsail_angle, rudder_angle, jib_angle, jib_side_flag): + """ + Send a properly formatted command packet to the Teensy. + Return 0 on success or 1 if encoding or writing fails. + :param mainsail_angle: new mainsail angle to set (integer). Should be in range [0, 90]. + :param rudder_angle: new rudder angle to set (integer). Should be in range [-45, 45]. + :param jib_angle: new jib angle to set (integer). Should be in range [10, 80]. + :param jib_side_flag: side to set the jib on (0 = port, 1 = starboard). + """ + try: + # Check bounds. For the sails, this is just defensive. + mainsail_angle = max(min(mainsail_angle, 127), -128) + rudder_angle = rudder_angle - PHYSICAL.RUDDER_MIN_ANGLE + jib_angle = max(min(jib_angle, 127), -128) + + # Convert control values to 8-bit integers (bytes). The else check is just defensive. + mainsail_byte = ( + mainsail_angle & 0xFF + if mainsail_angle >= 0 + else (mainsail_angle + 256) & 0xFF + ) + rudder_byte = ( + rudder_angle & 0xFF + if rudder_angle >= 0 + else (rudder_angle + 256) & 0xFF + ) + jib_angle_byte = ( + jib_angle & 0xFF if jib_angle >= 0 else (jib_angle + 256) & 0xFF + ) + jib_side_byte = jib_side_flag & 0xFF + + # Command payload format between start/end flags: + # [mainsail_angle, rudder_angle, jib_angle, jib_side_flag] + command_packet = bytearray( + [ + SERIAL.RX_START_FLAG, + mainsail_byte, + rudder_byte, + jib_angle_byte, + jib_side_byte, + SERIAL.RX_END_FLAG, + ] + ) + + # Send the packet over serial. + self.serial.write(command_packet) + return 0 + except Exception: + return 1 + + def read_telemetry(self, data): + """Read a telemetry packet into data; return 0 on success, otherwise 1. + + Firmware must send exactly SERIAL.TX_PACKET_LEN payload bytes between + SERIAL.TX_START_FLAG (0xFF) and SERIAL.TX_END_FLAG (0xEE). Neither flag value may + occur anywhere in the payload, including wind bytes and counters. + Payload order: [wind_hi, wind_lo, mainsail_angle, rudder_angle, + jib_angle, jib_side_flag, dropped_packets]. + + Partial packets persist between calls. After a successful read, + queued serial input is discarded, matching the original driver. + """ + # Check for waiting serial data. + while self.serial.in_waiting > 0: + incoming_byte = self.serial.read() + + # If we see a packet start byte: set flags, clear buffer. + if incoming_byte == SERIAL.TX_START_FLAG.to_bytes(1, "big"): + self.packet_started = True + self.buffer = [] + # If we see a packet end byte and the buffer is full, process the buffer and store into ``data``. + elif ( + self.packet_started + and incoming_byte == SERIAL.TX_END_FLAG.to_bytes(1, "big") + and len(self.buffer) == SERIAL.TX_PACKET_LEN + ): + self.packet_started = False + ( + data["wind_angle"], + data["mainsail_angle"], + data["rudder_angle"], + data["jib_angle"], + data["jib_side_flag"], + data["dropped_packets"], + ) = self._parse_packet(self.buffer) + self.serial.reset_input_buffer() # Clear buffer. + return 0 + # If we have previously seen a packet start byte, add data to our buffer. + elif self.packet_started: + self.buffer.append(int.from_bytes(incoming_byte, "big")) + return 1 + + def _parse_packet(self, packet): + """Decode the seven-byte telemetry payload, excluding framing flags.""" + wind_angle = (packet[0] << 8) | packet[1] + + # uint8_t to int8_t conversion for negative values. + mainsail_angle = packet[2] - 256 if packet[2] >= 128 else packet[2] + mainsail_angle = ( + PHYSICAL.MAINSAIL_MAX_ANGLE - mainsail_angle + ) # invert for mechanical swap + rudder_angle = packet[3] - 256 if packet[3] >= 128 else packet[3] + rudder_angle -= -PHYSICAL.RUDDER_MIN_ANGLE # undo the command offset + jib_angle = packet[4] - 256 if packet[4] >= 128 else packet[4] + jib_angle = ( + PHYSICAL.JIB_MAX_ANGLE + PHYSICAL.JIB_MIN_ANGLE - jib_angle + ) # invert for mechanical swap + + jib_side_flag = packet[5] + dropped_packets = packet[6] + + return ( + wind_angle, + mainsail_angle, + rudder_angle, + jib_angle, + jib_side_flag, + dropped_packets, + ) + + def close(self): + """Close the serial connection.""" + if hasattr(self, "serial"): + self.serial.close() diff --git a/src/sailing/sailing/teensy/teensy_node.py b/src/sailing/sailing/teensy/teensy_node.py new file mode 100644 index 0000000..48cd711 --- /dev/null +++ b/src/sailing/sailing/teensy/teensy_node.py @@ -0,0 +1,204 @@ +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import Int32, UInt8 + +from sailing.constants import PHYSICAL, TEENSY + +from . import serial_port + + +class Teensy(Node): + """ + ROS 2 Node that interfaces with a Teensy microcontroller to control and + monitor the boat's hardware (mainsail, rudder, and jib). It communicates + with the Teensy, reads telemetry (wind, angles, dropped packets), and sends + control commands from ROS subscriptions. + + Command topics (namespace-relative): ``mainsail_angle`` (Int32), ``rudder_angle`` + (Int32), ``jib_angle`` (Int32), ``jib_side_flag`` (UInt8: 0 = port side, + 1 = starboard side). + """ + + def __init__(self): + super().__init__("teensy") + + # Declare parameters. + self.declare_parameter("teensy_port", TEENSY.DEFAULT_PORT) + self.declare_parameter( + "rx_period", TEENSY.DEFAULT_TELEMETRY_POLL_PERIOD_SECONDS + ) + + # Read parameters. + self.telemetry_poll_period = float(self.get_parameter("rx_period").value) + self.teensy_port = self.get_parameter("teensy_port").value + + if self.telemetry_poll_period <= 0: + raise ValueError("Telemetry polling period must be positive.") + + # Initialize the hardware driver. + try: + self.serial_port = serial_port.SerialPort(self.teensy_port) + except Exception as exc: + self.get_logger().error( + f"Serial connection failed: port={self.teensy_port} reason={exc}" + ) + raise + self.get_logger().info( + f"Serial connection opened: port={self.teensy_port} " + f"poll_period={self.telemetry_poll_period}s" + ) + + # Initialize desired actuator positions. + self.desired_mainsail_angle = TEENSY.INITIAL_MAINSAIL_ANGLE + self.desired_rudder_angle = TEENSY.INITIAL_RUDDER_ANGLE + self.desired_jib_angle = TEENSY.INITIAL_JIB_ANGLE + self.desired_jib_side_flag = PHYSICAL.JIB_SIDE_PORT + + # Create telemetry publishers. + self.actual_mainsail_angle_pub = self.create_publisher( + Int32, "actual_mainsail_angle", 10 + ) + self.actual_rudder_angle_pub = self.create_publisher( + Int32, "actual_rudder_angle", 10 + ) + self.actual_jib_angle_pub = self.create_publisher(Int32, "actual_jib_angle", 10) + self.actual_jib_side_flag_pub = self.create_publisher( + UInt8, "actual_jib_side_flag", 10 + ) + self.dropped_packets_pub = self.create_publisher(Int32, "dropped_packets", 10) + + # Poll telemetry periodically. + self.timer = self.create_timer(self.telemetry_poll_period, self.check_telemetry) + + # Subscribe to actuator commands. + self.create_subscription( + Int32, "mainsail_angle", self.mainsail_angle_callback, 10 + ) + self.create_subscription(Int32, "rudder_angle", self.rudder_angle_callback, 10) + self.create_subscription(Int32, "jib_angle", self.jib_angle_callback, 10) + self.create_subscription( + UInt8, "jib_side_flag", self.jib_side_flag_callback, 10 + ) + + def mainsail_angle_callback(self, msg): + """Update the mainsail goal and send the full command.""" + self.desired_mainsail_angle = msg.data + self._send_command_to_teensy() + + def rudder_angle_callback(self, msg): + """Update the rudder goal and send the full command.""" + self.desired_rudder_angle = msg.data + self._send_command_to_teensy() + + def jib_angle_callback(self, msg): + """Update the jib goal and send the full command.""" + self.desired_jib_angle = msg.data + self._send_command_to_teensy() + + def jib_side_flag_callback(self, msg): + """Update the jib side and send the full command.""" + self.desired_jib_side_flag = int(msg.data) + self._send_command_to_teensy() + + def check_telemetry(self): + """Read and publish telemetry from the Teensy.""" + data = {} + try: + received = self.serial_port.read_telemetry(data) == 0 + except OSError as exc: + self.get_logger().warning( + f"Telemetry read failed: reason={exc}", + throttle_duration_sec=TEENSY.WARNING_THROTTLE_SECONDS, + ) + return + + if received: + mainsail_angle_msg = Int32() + mainsail_angle_msg.data = data["mainsail_angle"] + self.actual_mainsail_angle_pub.publish(mainsail_angle_msg) + + rudder_angle_msg = Int32() + rudder_angle_msg.data = data["rudder_angle"] + self.actual_rudder_angle_pub.publish(rudder_angle_msg) + + jib_angle_msg = Int32() + jib_angle_msg.data = data["jib_angle"] + self.actual_jib_angle_pub.publish(jib_angle_msg) + + jib_side_flag_msg = UInt8() + jib_side_flag_msg.data = data["jib_side_flag"] + self.actual_jib_side_flag_pub.publish(jib_side_flag_msg) + + dropped_packets_msg = Int32() + dropped_packets_msg.data = data["dropped_packets"] + self.dropped_packets_pub.publish(dropped_packets_msg) + + side = ( + "port" + if data["jib_side_flag"] == PHYSICAL.JIB_SIDE_PORT + else "starboard" + ) + self.get_logger().debug( + f"RX mainsail={data['mainsail_angle']} rudder={data['rudder_angle']} " + f"jib={data['jib_angle']} jib_side={side} " + f"dropped_packets={data['dropped_packets']}" + ) + else: + self.get_logger().debug("No telemetry received.") + + def _send_command_to_teensy(self): + """Send the latest mainsail, rudder, and jib goals in one packet.""" + side = ( + "port" + if self.desired_jib_side_flag == PHYSICAL.JIB_SIDE_PORT + else "starboard" + ) + command = ( + f"mainsail={self.desired_mainsail_angle} rudder={self.desired_rudder_angle} " + f"jib={self.desired_jib_angle} jib_side={side}" + ) + try: + result = self.serial_port.send_command( + self.desired_mainsail_angle, + self.desired_rudder_angle, + self.desired_jib_angle, + self.desired_jib_side_flag, + ) + except (OSError, ValueError) as exc: + reason = str(exc) + else: + if result == 0: + self.get_logger().debug(f"TX {command}") + return + reason = f"driver returned status {result}" + self.get_logger().warning( + f"Command failed: {command} reason={reason}", + throttle_duration_sec=TEENSY.WARNING_THROTTLE_SECONDS, + ) + + def destroy_node(self): + """Close the serial connection and release ROS resources.""" + try: + self.serial_port.close() + finally: + result = super().destroy_node() + return result + + +def main(args=None): + """Run the Teensy node and release resources on exit.""" + rclpy.init(args=args) + teensy_node = Teensy() + + try: + rclpy.spin(teensy_node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + teensy_node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/sailing/setup.cfg b/src/sailing/setup.cfg new file mode 100644 index 0000000..8d26e60 --- /dev/null +++ b/src/sailing/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sailing +[install] +install_scripts=$base/lib/sailing diff --git a/src/sailing/setup.py b/src/sailing/setup.py new file mode 100644 index 0000000..5973629 --- /dev/null +++ b/src/sailing/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup + +package_name = "sailing" + +setup( + name=package_name, + version="0.0.1", + packages=find_packages(include=[package_name, f"{package_name}.*"]), + data_files=[ + ( + "share/ament_index/resource_index/packages", + [f"resource/{package_name}"], + ), + (f"share/{package_name}", ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="ericcai32", + maintainer_email="ericcai32@gmail.com", + description="Sailing control and hardware interfaces.", + license="Apache License 2.0", + entry_points={"console_scripts": ["teensy_node = sailing.teensy.teensy_node:main"]}, +) diff --git a/src/sailing/test/conftest.py b/src/sailing/test/conftest.py new file mode 100644 index 0000000..6740a40 --- /dev/null +++ b/src/sailing/test/conftest.py @@ -0,0 +1,52 @@ +"""Fake serial connection shared by driver tests.""" + +from unittest.mock import Mock + +import pytest +from sailing.teensy import serial_port + + +class FakeSerial: + def __init__(self): + self.incoming = bytearray() + self.written = bytearray() + self.is_open = True + self.write_error = None + self.read_error = None + + @property + def in_waiting(self): + if self.read_error: + raise self.read_error + return len(self.incoming) + + def read(self, size=1): + data = bytes(self.incoming[:size]) + del self.incoming[:size] + return data + + def write(self, data): + if self.write_error: + raise self.write_error + self.written.extend(data) + return len(data) + + def reset_input_buffer(self): + self.incoming.clear() + + def close(self): + self.is_open = False + + +@pytest.fixture +def transport(monkeypatch): + transport = FakeSerial() + monkeypatch.setattr(serial_port.serial, "Serial", Mock(return_value=transport)) + return transport + + +@pytest.fixture +def driver(transport): + driver = serial_port.SerialPort("/dev/test-teensy") + yield driver + driver.close() diff --git a/src/sailing/test/test_serial_port.py b/src/sailing/test/test_serial_port.py new file mode 100644 index 0000000..3dd5dfa --- /dev/null +++ b/src/sailing/test/test_serial_port.py @@ -0,0 +1,112 @@ +"""Regression tests for the migrated serial driver's legacy behavior. + +Cover framing, sail telemetry inversions, rudder wire center 45, command +encoding, error handling, and cleanup using a fake serial connection. +Telemetry payloads must exclude start and end flag values. These tests +preserve the migration's behavior; they do not verify hardware calibration. +""" + +import pytest +from sailing.constants import PHYSICAL +from sailing.teensy import serial_port + + +def frame(wind=300, mainsail=20, rudder=45, jib=30, side=1, dropped=7): + return bytes( + [0xFF, wind >> 8, wind & 0xFF, mainsail, rudder, jib, side, dropped, 0xEE] + ) + + +def test_open_uses_configured_port_and_baudrate(transport): + serial_port.SerialPort("/dev/test-teensy") + args, kwargs = serial_port.serial.Serial.call_args + assert (args[0] if args else kwargs["port"]) == "/dev/test-teensy" + assert kwargs["baudrate"] == 9600 + + +def test_open_failure_is_visible(transport): + serial_port.serial.Serial.side_effect = OSError("disconnected") + with pytest.raises(OSError, match="disconnected"): + serial_port.SerialPort("/dev/test-teensy") + + +def test_empty_read_leaves_data_unchanged(driver): + data = {"previous": 123} + assert driver.read_telemetry(data) == 1 + assert data == {"previous": 123} + + +def test_decode_telemetry(driver, transport): + transport.incoming.extend(frame()) + data = {} + assert driver.read_telemetry(data) == 0 + # Preserve the original mechanical-swap inversions for both sails. + assert data == dict( + wind_angle=300, + mainsail_angle=70, + rudder_angle=0, + jib_angle=60, + jib_side_flag=1, + dropped_packets=7, + ) + + +@pytest.mark.parametrize("split", range(1, 9)) +def test_fragmented_frame(driver, transport, split): + packet = frame() + transport.incoming.extend(packet[:split]) + data = {"previous": 123} + assert driver.read_telemetry(data) == 1 + assert data == {"previous": 123} + transport.incoming.extend(packet[split:]) + assert driver.read_telemetry(data) == 0 + assert data["wind_angle"] == 300 + + +@pytest.mark.parametrize( + "garbage", [b"noise", frame()[:-1] + b"\x00", b"\xff" + b"\x01" * 64] +) +def test_resynchronize_after_noise_or_bad_frame(driver, transport, garbage): + transport.incoming.extend(garbage + frame()) + data = {} + for _ in range(3): + if driver.read_telemetry(data) == 0: + break + assert data.get("wind_angle") == 300 + + +@pytest.mark.parametrize("position", ["minimum", "neutral", "maximum"]) +def test_command_bytes(driver, transport, position): + rudder = { + "minimum": PHYSICAL.RUDDER_MIN_ANGLE, + "neutral": 0, + "maximum": PHYSICAL.RUDDER_MAX_ANGLE, + }[position] + # The migrated protocol uses 45 as the rudder wire center. + wire = rudder + 45 + side = PHYSICAL.JIB_SIDE_STB + assert driver.send_command(20, rudder, 30, side) == 0 + assert transport.written == bytes([255, 20, wire, 30, side, 238]) + + +@pytest.mark.parametrize("side", [-1, 2]) +def test_side_is_encoded_as_unsigned_byte(driver, transport, side): + assert driver.send_command(20, 0, 30, side) == 0 + assert transport.written == bytes([255, 20, 45, 30, side & 255, 238]) + + +def test_read_failure_is_visible(driver, transport): + transport.read_error = OSError("unplugged") + with pytest.raises(OSError, match="unplugged"): + driver.read_telemetry({}) + + +def test_write_failure_returns_failure_status(driver, transport): + transport.write_error = OSError("unplugged") + assert driver.send_command(20, 0, 30, 0) == 1 + assert not transport.written + + +def test_close_closes_connection(driver, transport): + driver.close() + assert not transport.is_open diff --git a/src/sailing/test/test_teensy_node.py b/src/sailing/test/test_teensy_node.py new file mode 100644 index 0000000..fad5caa --- /dev/null +++ b/src/sailing/test/test_teensy_node.py @@ -0,0 +1,207 @@ +"""Real ROS tests with only the hardware driver faked.""" + +import time +import uuid +from dataclasses import replace +from unittest.mock import Mock, call + +import pytest +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from sailing.teensy import teensy_node +from std_msgs.msg import Int32, UInt8 + + +@pytest.fixture +def ros(): + rclpy.init(args=["--ros-args", "-r", "__ns:=/test_" + uuid.uuid4().hex]) + yield + rclpy.shutdown() + + +@pytest.fixture +def ros_node(ros, monkeypatch): + hardware = Mock( + send_command=Mock(return_value=0), read_telemetry=Mock(return_value=1) + ) + monkeypatch.setattr( + teensy_node.serial_port, "SerialPort", Mock(return_value=hardware) + ) + node = teensy_node.Teensy() + peer = Node("test_peer") + executor = SingleThreadedExecutor() + executor.add_node(node) + executor.add_node(peer) + yield node, peer, hardware, executor + executor.shutdown() + peer.destroy_node() + # Cleanup also works after the explicit close-failure test. + hardware.close.side_effect = None + node.destroy_node() + + +def spin_until(executor, predicate): + deadline = time.monotonic() + 10 + while not predicate() and time.monotonic() < deadline: + executor.spin_once(timeout_sec=0.05) + assert predicate(), "Timed out waiting for ROS" + + +def observe_telemetry(node, peer, executor): + received = {} + topics = { + "actual_mainsail_angle": Int32, + "actual_rudder_angle": Int32, + "actual_jib_angle": Int32, + "actual_jib_side_flag": UInt8, + "dropped_packets": Int32, + } + subscriptions = [ + peer.create_subscription( + message_type, + topic, + lambda msg, topic=topic: received.setdefault(topic, []).append(msg.data), + 10, + ) + for topic, message_type in topics.items() + ] + spin_until( + executor, + lambda: all( + peer.count_publishers(subscription.topic_name) == 1 + and node.count_subscribers(subscription.topic_name) == 1 + for subscription in subscriptions + ), + ) + return received + + +def test_command_topics_send_full_latest_command(ros_node): + node, peer, hardware, executor = ros_node + node.timer.cancel() + commands = [ + ("mainsail_angle", Int32, 20), + ("rudder_angle", Int32, -10), + ("jib_angle", Int32, 30), + ("jib_side_flag", UInt8, 1), + ] + for count, (topic, message_type, value) in enumerate(commands, start=1): + publisher = peer.create_publisher(message_type, topic, 10) + spin_until(executor, lambda: publisher.get_subscription_count() == 1) + publisher.publish(message_type(data=value)) + spin_until(executor, lambda: hardware.send_command.call_count >= count) + assert hardware.send_command.call_args_list == [ + call(20, 0, 0, 0), + call(20, -10, 0, 0), + call(20, -10, 30, 0), + call(20, -10, 30, 1), + ] + + +def test_timer_publishes_complete_telemetry(ros_node): + node, peer, hardware, executor = ros_node + received = observe_telemetry(node, peer, executor) + sample = dict( + wind_angle=300, + mainsail_angle=20, + rudder_angle=-10, + jib_angle=30, + jib_side_flag=1, + dropped_packets=7, + ) + + def read(data): + data.update(sample) + return 0 + + hardware.read_telemetry.side_effect = read + expected = { + "actual_mainsail_angle": 20, + "actual_rudder_angle": -10, + "actual_jib_angle": 30, + "actual_jib_side_flag": 1, + "dropped_packets": 7, + } + spin_until(executor, lambda: set(received) == set(expected)) + assert {topic: values[-1] for topic, values in received.items()} == expected + + +@pytest.mark.parametrize("error", [None, OSError("unplugged")]) +def test_no_publication_on_missing_or_failed_read(ros_node, monkeypatch, error): + node, peer, hardware, executor = ros_node + node.timer.cancel() + received = observe_telemetry(node, peer, executor) + logger = Mock(wraps=node.get_logger()) + monkeypatch.setattr(node, "get_logger", lambda: logger) + hardware.read_telemetry.side_effect = error + node.check_telemetry() + deadline = time.monotonic() + 0.2 + spin_until(executor, lambda: time.monotonic() >= deadline) + assert not received + if error: + logger.warning.assert_called_once() + assert logger.warning.call_args.kwargs["throttle_duration_sec"] > 0 + else: + logger.warning.assert_not_called() + + +@pytest.mark.parametrize( + "error", [None, OSError("unplugged"), ValueError("invalid angle")] +) +def test_command_failure_is_reported(ros_node, monkeypatch, error): + node, _, hardware, _ = ros_node + logger = Mock(wraps=node.get_logger()) + monkeypatch.setattr(node, "get_logger", lambda: logger) + hardware.send_command.return_value = 1 + hardware.send_command.side_effect = error + node.rudder_angle_callback(Int32(data=10)) + logger.warning.assert_called_once() + assert "Command failed" in logger.warning.call_args.args[0] + if error: + assert str(error) in logger.warning.call_args.args[0] + + +@pytest.mark.parametrize("period", [0, -0.1]) +def test_invalid_poll_period_does_not_open_port(ros, monkeypatch, period): + monkeypatch.setattr( + teensy_node, + "TEENSY", + replace(teensy_node.TEENSY, DEFAULT_TELEMETRY_POLL_PERIOD_SECONDS=period), + ) + factory = Mock() + monkeypatch.setattr(teensy_node.serial_port, "SerialPort", factory) + node = teensy_node.Teensy.__new__(teensy_node.Teensy) + try: + with pytest.raises(ValueError): + node.__init__() + factory.assert_not_called() + finally: + Node.destroy_node(node) + + +def test_serial_open_failure_propagates(ros, monkeypatch): + monkeypatch.setattr( + teensy_node.serial_port, "SerialPort", Mock(side_effect=OSError("missing port")) + ) + node = teensy_node.Teensy.__new__(teensy_node.Teensy) + try: + with pytest.raises(OSError, match="missing port"): + node.__init__() + finally: + Node.destroy_node(node) + + +@pytest.mark.parametrize("error", [None, OSError("close failed")]) +def test_shutdown_releases_ros_even_if_serial_close_fails(ros_node, error): + node, _, hardware, _ = ros_node + hardware.close.side_effect = error + if error: + with pytest.raises(OSError, match="close failed"): + node.destroy_node() + else: + node.destroy_node() + hardware.close.assert_called_once() + assert not list(node.publishers) + assert not list(node.subscriptions) + assert not list(node.timers)