diff --git a/models/posim_robot_models/CMakeLists.txt b/models/posim_robot_models/CMakeLists.txt index 41d5078b..fc373f83 100644 --- a/models/posim_robot_models/CMakeLists.txt +++ b/models/posim_robot_models/CMakeLists.txt @@ -19,4 +19,13 @@ install( ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test( + test_manual_control_buttons test/test_manual_control_buttons.py) + ament_add_pytest_test( + test_manual_control_services test/test_manual_control_services.py) + ament_add_pytest_test(test_ws_lifecycle test/test_ws_lifecycle.py) +endif() + ament_package() diff --git a/models/posim_robot_models/package.xml b/models/posim_robot_models/package.xml index ffba9b85..728fb36d 100644 --- a/models/posim_robot_models/package.xml +++ b/models/posim_robot_models/package.xml @@ -16,6 +16,7 @@ rclpy sensor_msgs std_msgs + ament_cmake_pytest ament_cmake diff --git a/models/posim_robot_models/scripts/ardusub_manual_control.py b/models/posim_robot_models/scripts/ardusub_manual_control.py index df129a79..28e68eec 100755 --- a/models/posim_robot_models/scripts/ardusub_manual_control.py +++ b/models/posim_robot_models/scripts/ardusub_manual_control.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import math +import time import rclpy from mavros_msgs.msg import ManualControl, State @@ -25,6 +26,7 @@ DEADZONE_HEAVE = 0.18 RATE_HZ = 20.0 TIMEOUT_SEC = 0.3 +PENDING_COMMAND_TTL_SEC = 2.0 STABILIZE_MODE = "STABILIZE" DEPTH_HOLD_MODE = "ALT_HOLD" @@ -116,26 +118,22 @@ def __init__(self): }, } self.last_axes = [] - self.last_buttons = [] self.connected = False self.armed = False self.current_mode = "" - self.last_requested_mode = "" self.throttle_index = THROTTLE_DEFAULT_INDEX self.throttle_scale = THROTTLE_LEVELS[self.throttle_index] - self._last_arm_on = 0 - self._last_arm_off = 0 - self._last_hold_on = 0 - self._last_hold_off = 0 - self._last_scale_up = 0 - self._last_scale_dn = 0 - self._last_warn_sec = {} self._mode_future = None self._arm_future = None + self._mode_in_flight = None + self._arm_in_flight = None + self._pending_mode = None + self._pending_disarm = False + self._disarm_warn_at = None self._publish_armed_state() self.timer = self.create_timer(1.0 / RATE_HZ, self.tick) @@ -173,12 +171,17 @@ def cb_state(self, msg): def _update_input_state(self, source, msg): state = self.input_state[source] + previous_buttons = state["buttons"] state["axes"] = list(msg.axes) state["buttons"] = list(msg.buttons) if any(abs(axis) > 1e-6 for axis in state["axes"]) or any(state["buttons"]): state["activity_time"] = self.get_clock().now() + # A keyboard pulse can be pressed and released between two 20 Hz ticks. + # Consume button edges here, keeping history separate for each source. + self._handle_button_edges(previous_buttons, state["buttons"]) + def _select_active_input(self): now = self.get_clock().now() active_state = None @@ -202,9 +205,6 @@ def _select_active_input(self): def _get_axis(self, idx): return float(self.last_axes[idx]) if idx < len(self.last_axes) else 0.0 - def _get_btn(self, idx): - return int(self.last_buttons[idx]) if idx < len(self.last_buttons) else 0 - def _publish_armed_state(self): msg = Bool() msg.data = self.armed @@ -226,116 +226,156 @@ def _warn_throttled(self, key, message, period_sec=2.0): now_sec = self.get_clock().now().nanoseconds * 1e-9 last_sec = self._last_warn_sec.get(key, -math.inf) if now_sec - last_sec >= period_sec: - self.get_logger().warn(message) + self.get_logger().warning(message) self._last_warn_sec[key] = now_sec + def _expire_pending_commands(self): + now = time.monotonic() + if self._pending_mode is not None and now >= self._pending_mode[1]: + self._pending_mode = None + self.get_logger().warning("Pending mode request expired; press the mode button again") + if self._disarm_warn_at is not None and now >= self._disarm_warn_at: + self._disarm_warn_at = None + self.get_logger().warning( + "Disarm still pending; waiting for the earlier arming response" + ) + def _call_set_mode(self, mode): - self.last_requested_mode = mode + self._expire_pending_commands() + if self._mode_future is not None: + # Keep only the latest intent, not a backlog of mode changes. + self._pending_mode = ( + None + if mode == self._mode_in_flight + else (mode, time.monotonic() + PENDING_COMMAND_TTL_SEC) + ) + return if not self.mode_client.service_is_ready(): - self._warn_throttled("set_mode", "Waiting for MAVROS set_mode service") - return - if self._mode_future is not None and not self._mode_future.done(): + self._warn_throttled( + "set_mode", "Waiting for MAVROS set_mode service; press the mode button again" + ) return request = SetMode.Request() request.custom_mode = mode - self._mode_future = self.mode_client.call_async(request) - self._mode_future.add_done_callback(self._on_mode_response) + try: + future = self.mode_client.call_async(request) + except Exception as exc: + self.get_logger().warning(f"Could not send mode request {mode}: {exc}") + return + self._mode_in_flight = mode + self._mode_future = future + future.add_done_callback(lambda completed: self._on_mode_response(completed, mode)) - def _on_mode_response(self, future): + def _on_mode_response(self, future, mode): + if future is not self._mode_future: + return + self._mode_future = None + self._mode_in_flight = None try: response = future.result() + if response.mode_sent: + self.get_logger().info(f"Requested ArduSub mode: {mode}") + else: + self.get_logger().warning(f"ArduSub rejected mode request: {mode}") except Exception as exc: - self.get_logger().warn(f"Set mode request failed: {exc}") - return + self.get_logger().warning(f"Set mode request {mode} failed: {exc}") - if response.mode_sent: - self.get_logger().info(f"Requested ArduSub mode: {self.last_requested_mode}") - else: - self.get_logger().warn(f"ArduSub rejected mode request: {self.last_requested_mode}") + self._expire_pending_commands() + pending = self._pending_mode + self._pending_mode = None + if pending is not None: + self._call_set_mode(pending[0]) def _call_arm(self, arm): - if not self.arm_client.service_is_ready(): - self._warn_throttled("arming", "Waiting for MAVROS arming service") + self._expire_pending_commands() + if self._arm_future is not None: + if arm: + # Never arm later as a side effect of an older request completing. + self._warn_throttled( + "arm_busy", + "Arming request pending; arm was not queued. Press again when ready", + ) + elif self._arm_in_flight and not self._pending_disarm: + # Do not expire disarm: the older arm request may still complete late. + self._pending_disarm = True + self._disarm_warn_at = time.monotonic() + PENDING_COMMAND_TTL_SEC return - if self._arm_future is not None and not self._arm_future.done(): + + if not self.arm_client.service_is_ready(): + self._warn_throttled( + "arming", "Waiting for MAVROS arming service; press the arm/disarm button again" + ) return request = CommandBool.Request() request.value = arm - self._arm_future = self.arm_client.call_async(request) - self._arm_future.add_done_callback(self._on_arm_response) - - def _on_arm_response(self, future): try: - response = future.result() + future = self.arm_client.call_async(request) except Exception as exc: - self.get_logger().warn(f"Arm request failed: {exc}") + self.get_logger().warning(f"Could not send arm={arm} request: {exc}") return + self._arm_in_flight = arm + self._arm_future = future + future.add_done_callback(lambda completed: self._on_arm_response(completed, arm)) - if not response.success: - self.get_logger().warn("ArduSub rejected arm/disarm request") - - def _update_throttle_scale(self): - scale_up = self._get_btn(BTN_SCALE_UP) - scale_dn = self._get_btn(BTN_SCALE_DN) + def _on_arm_response(self, future, arm): + if future is not self._arm_future: + return + self._arm_future = None + self._arm_in_flight = None + try: + response = future.result() + if not response.success: + self.get_logger().warning(f"ArduSub rejected arm={arm} request") + except Exception as exc: + self.get_logger().warning(f"Arm request arm={arm} failed: {exc}") - if scale_up == 1 and self._last_scale_up == 0: - if self.throttle_index < len(THROTTLE_LEVELS) - 1: - self.throttle_index += 1 - self.throttle_scale = THROTTLE_LEVELS[self.throttle_index] - self.get_logger().info(f"manual XY/yaw scale = {self.throttle_scale:.2f}") + self._expire_pending_commands() + disarm_pending = self._pending_disarm + self._pending_disarm = False + self._disarm_warn_at = None + if disarm_pending: + self._call_arm(False) - if scale_dn == 1 and self._last_scale_dn == 0: - if self.throttle_index > 0: - self.throttle_index -= 1 - self.throttle_scale = THROTTLE_LEVELS[self.throttle_index] - self.get_logger().info(f"manual XY/yaw scale = {self.throttle_scale:.2f}") + def _handle_button_edges(self, previous, current): + def down(buttons, index): + return index < len(buttons) and buttons[index] == 1 - self._last_scale_up = scale_up - self._last_scale_dn = scale_dn + def pressed(index): + return down(current, index) and not down(previous, index) - def _handle_mode_buttons(self): - hold_on = self._get_btn(BTN_Z_HOLD_ON) - hold_off = self._get_btn(BTN_Z_HOLD_OFF) + # Disarm takes precedence if a message contains both arm buttons. + if pressed(BTN_ARM_OFF): + self._call_arm(False) + elif pressed(BTN_ARM_ON) and not down(current, BTN_ARM_OFF): + self._call_arm(True) - if hold_on == 1 and self._last_hold_on == 0: + if pressed(BTN_Z_HOLD_ON): self._call_set_mode(DEPTH_HOLD_MODE) - - if hold_off == 1 and self._last_hold_off == 0: + if pressed(BTN_Z_HOLD_OFF): self._call_set_mode(STABILIZE_MODE) - self._last_hold_on = hold_on - self._last_hold_off = hold_off - - def _handle_arm_buttons(self): - arm_on = self._get_btn(BTN_ARM_ON) - arm_off = self._get_btn(BTN_ARM_OFF) - - if arm_on == 1 and self._last_arm_on == 0: - self._call_arm(True) - - if arm_off == 1 and self._last_arm_off == 0: - self._call_arm(False) + if pressed(BTN_SCALE_UP) and self.throttle_index < len(THROTTLE_LEVELS) - 1: + self.throttle_index += 1 + self.throttle_scale = THROTTLE_LEVELS[self.throttle_index] + self.get_logger().info(f"manual XY/yaw scale = {self.throttle_scale:.2f}") - self._last_arm_on = arm_on - self._last_arm_off = arm_off + if pressed(BTN_SCALE_DN) and self.throttle_index > 0: + self.throttle_index -= 1 + self.throttle_scale = THROTTLE_LEVELS[self.throttle_index] + self.get_logger().info(f"manual XY/yaw scale = {self.throttle_scale:.2f}") def tick(self): + self._expire_pending_commands() active_input = self._select_active_input() if active_input is None: self.last_axes = [] - self.last_buttons = [] self._publish_manual(0.0, 0.0, THROTTLE_NEUTRAL, 0.0) return self.last_axes = active_input["axes"] - self.last_buttons = active_input["buttons"] - - self._handle_arm_buttons() - self._handle_mode_buttons() - self._update_throttle_scale() forward = -dz(self._get_axis(AXIS_FWD), DEADZONE) * MAX_MANUAL forward *= self.throttle_scale diff --git a/models/posim_robot_models/scripts/ws_to_joy.py b/models/posim_robot_models/scripts/ws_to_joy.py index 8de1f638..dec1c8aa 100755 --- a/models/posim_robot_models/scripts/ws_to_joy.py +++ b/models/posim_robot_models/scripts/ws_to_joy.py @@ -1,11 +1,15 @@ #!/usr/bin/env python3 import asyncio import json +import signal import rclpy from rclpy.node import Node +from rclpy.executors import ExternalShutdownException +from rclpy.signals import SignalHandlerOptions from sensor_msgs.msg import Joy import websockets +from websockets.exceptions import ConnectionClosed class JoyWebSocketServer(Node): @@ -45,37 +49,71 @@ def publish_payload(self, payload): self.pub.publish(msg) -async def main_async(): - rclpy.init() - node = JoyWebSocketServer() - - async def handler(websocket): - node.get_logger().info("Web joystick client connected") - +async def relay_messages(websocket, node, stopping): + """Relay live input only while both the server and ROS context are active.""" + node.get_logger().info("Web joystick client connected") + try: async for message in websocket: + # Give signal/close callbacks a turn even when recv() drains a backlog. + await asyncio.sleep(0) + if stopping.is_set() or not rclpy.ok(): + break try: payload = json.loads(message) - except Exception as exc: - node.get_logger().warn(f"Invalid websocket JSON payload: {exc}") + except (ValueError, TypeError) as exc: + node.get_logger().warning(f"Invalid websocket JSON payload: {exc}") continue - try: node.publish_payload(payload) except Exception as exc: + if stopping.is_set() or not rclpy.ok(): + break node.get_logger().error(f"Failed to publish Joy payload: {exc}") + except ConnectionClosed: + # A dropped browser connection is expected; the listener stays available. + pass - async with websockets.serve(handler, node.ws_host, node.ws_port): - while rclpy.ok(): - rclpy.spin_once(node, timeout_sec=0.01) - await asyncio.sleep(0.01) - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() +async def main_async(): + # Close websocket handlers before invalidating their ROS publisher context. + rclpy.init(signal_handler_options=SignalHandlerOptions.NO) + node = None + stopping = asyncio.Event() + loop = asyncio.get_running_loop() + previous_handlers = {} + try: + node = JoyWebSocketServer() + for signum in (signal.SIGINT, signal.SIGTERM): + previous_handlers[signum] = signal.getsignal(signum) + loop.add_signal_handler(signum, stopping.set) + + async def handler(websocket): + await relay_messages(websocket, node, stopping) + + async with websockets.serve(handler, node.ws_host, node.ws_port, close_timeout=2): + try: + while not stopping.is_set() and rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0.01) + await asyncio.sleep(0.01) + except ExternalShutdownException: + if rclpy.ok(): + raise + finally: + stopping.set() + finally: + for signum, previous in previous_handlers.items(): + loop.remove_signal_handler(signum) + signal.signal(signum, previous) + if node is not None: + node.destroy_node() + rclpy.try_shutdown() def main(): - asyncio.run(main_async()) + try: + asyncio.run(main_async()) + except KeyboardInterrupt: + pass if __name__ == "__main__": diff --git a/models/posim_robot_models/test/test_manual_control_buttons.py b/models/posim_robot_models/test/test_manual_control_buttons.py new file mode 100644 index 00000000..22b5b9ad --- /dev/null +++ b/models/posim_robot_models/test/test_manual_control_buttons.py @@ -0,0 +1,168 @@ +"""Regression checks for Joy button edges and independent axis sampling.""" + +import importlib.util +import os +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest +import rclpy +from sensor_msgs.msg import Joy + +SCRIPT = Path(__file__).parents[1] / "scripts" / "ardusub_manual_control.py" +spec = importlib.util.spec_from_file_location( + "manual_control_under_test", os.environ.get("POSIM_MANUAL_CONTROL_UNDER_TEST", SCRIPT) +) +manual = importlib.util.module_from_spec(spec) +spec.loader.exec_module(manual) + + +class TimePoint: + def __init__(self, seconds): + self.nanoseconds = int(seconds * 1e9) + + def __sub__(self, other): + return SimpleNamespace(nanoseconds=self.nanoseconds - other.nanoseconds) + + +class Clock: + seconds = 0.0 + + def now(self): + return TimePoint(self.seconds) + + +@pytest.fixture +def control(): + rclpy.init() + node = manual.ArduSubManualControl() + node.timer.cancel() + clock = Clock() + node.get_clock = lambda: clock + node._call_arm = Mock() + node._call_set_mode = Mock() + node._publish_manual = Mock() + yield node, clock + node.destroy_node() + rclpy.shutdown() + + +def joy(buttons=(), axes=()): + msg = Joy() + msg.axes = list(axes) + msg.buttons = [int(i in buttons) for i in range(17)] + return msg + + +@pytest.mark.parametrize("source", ["joystick", "keyboard"]) +@pytest.mark.parametrize( + "button,method,value", + [ + (manual.BTN_ARM_ON, "_call_arm", True), + (manual.BTN_ARM_OFF, "_call_arm", False), + (manual.BTN_Z_HOLD_ON, "_call_set_mode", "ALT_HOLD"), + (manual.BTN_Z_HOLD_OFF, "_call_set_mode", "STABILIZE"), + ], +) +def test_press_and_release_between_timer_ticks(control, source, button, method, value): + node, _ = control + node._update_input_state(source, joy([button])) + node._update_input_state(source, joy()) + node.tick() + getattr(node, method).assert_called_once_with(value) + + +def test_held_button_and_idle_other_source_do_not_repeat(control): + node, clock = control + for _ in range(20): + node.cb_joy(joy([manual.BTN_ARM_ON])) + node.cb_keyboard_joy(joy()) + node.tick() + clock.seconds = 1.0 + node.tick() + node.cb_joy(joy([manual.BTN_ARM_ON])) + node._call_arm.assert_called_once_with(True) + node.cb_joy(joy()) + node.cb_joy(joy([manual.BTN_ARM_ON])) + assert node._call_arm.call_count == 2 + + +def test_button_history_is_per_source(control): + node, _ = control + for _ in range(3): + node.cb_joy(joy([manual.BTN_Z_HOLD_OFF])) + node.cb_keyboard_joy(joy([manual.BTN_Z_HOLD_OFF])) + assert node._call_set_mode.call_count == 2 + node.cb_keyboard_joy(joy()) + node.cb_keyboard_joy(joy([manual.BTN_Z_HOLD_OFF])) + assert node._call_set_mode.call_count == 3 + + +def test_rapid_separate_presses_are_not_collapsed(control): + node, _ = control + for _ in range(5): + node.cb_keyboard_joy(joy([manual.BTN_Z_HOLD_OFF])) + node.cb_keyboard_joy(joy()) + assert node._call_set_mode.call_count == 5 + node.tick() + assert node._call_set_mode.call_count == 5 + + +def test_disarm_wins_when_both_arm_buttons_are_pressed(control): + node, _ = control + node.cb_joy(joy([manual.BTN_ARM_ON, manual.BTN_ARM_OFF])) + node._call_arm.assert_called_once_with(False) + + +def test_scale_uses_edges_and_stays_bounded(control): + node, _ = control + for _ in range(20): + node.cb_joy(joy([manual.BTN_SCALE_UP])) + assert node.throttle_scale == 0.75 + for _ in range(20): + node.cb_joy(joy()) + node.cb_joy(joy([manual.BTN_SCALE_UP])) + assert node.throttle_scale == 1.0 + for _ in range(20): + node.cb_joy(joy()) + node.cb_joy(joy([manual.BTN_SCALE_DN])) + assert node.throttle_scale == 0.25 + + +def test_empty_short_and_unmapped_buttons(control): + node, _ = control + node.cb_joy(Joy()) + node.cb_joy(joy([16])) + node.cb_joy(Joy()) + node._call_arm.assert_not_called() + node._call_set_mode.assert_not_called() + + +def test_axes_and_timeout_are_unchanged(control): + node, clock = control + node.cb_keyboard_joy(joy(axes=[0.0, -0.8, 0.0, 0.0, 0.0, 0.0])) + node.tick() + assert node._publish_manual.call_args.args == pytest.approx((400.0, 0.0, 500.0, 0.0)) + clock.seconds = 0.1 + node.cb_joy(joy(axes=[0.0, 0.8, 0.0, 0.0, 0.0, 0.0])) + node.tick() + assert node._publish_manual.call_args.args == pytest.approx((-400.0, 0.0, 500.0, 0.0)) + clock.seconds = 0.5 + node.tick() + node._publish_manual.assert_called_with(0.0, 0.0, 500.0, 0.0) + + +def test_axis_release_does_not_wait_for_timeout(control): + node, _ = control + node.cb_keyboard_joy(joy(axes=[0.0, -0.8, 0.0, 0.0, 0.0, 0.0])) + node.cb_keyboard_joy(joy()) + node.tick() + node._publish_manual.assert_called_with(0.0, 0.0, 500.0, 0.0) + + +def test_held_disarm_blocks_a_new_arm_edge(control): + node, _ = control + node.cb_joy(joy([manual.BTN_ARM_OFF])) + node.cb_joy(joy([manual.BTN_ARM_OFF, manual.BTN_ARM_ON])) + node._call_arm.assert_called_once_with(False) diff --git a/models/posim_robot_models/test/test_manual_control_services.py b/models/posim_robot_models/test/test_manual_control_services.py new file mode 100644 index 00000000..13130450 --- /dev/null +++ b/models/posim_robot_models/test/test_manual_control_services.py @@ -0,0 +1,222 @@ +"""Bounded command handoff, failure handling and request-specific logging.""" + +import importlib.util +import os +from concurrent.futures import Future +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest +import rclpy + +SCRIPT = Path(__file__).parents[1] / "scripts" / "ardusub_manual_control.py" +spec = importlib.util.spec_from_file_location( + "manual_services_under_test", os.environ.get("POSIM_MANUAL_CONTROL_UNDER_TEST", SCRIPT) +) +manual = importlib.util.module_from_spec(spec) +spec.loader.exec_module(manual) + + +class Client: + def __init__(self, field): + self.field = field + self.ready = True + self.values = [] + self.futures = [] + self.send_error = None + + def service_is_ready(self): + return self.ready + + def call_async(self, request): + if self.send_error: + raise self.send_error + self.values.append(getattr(request, self.field)) + future = Future() + self.futures.append(future) + return future + + +@pytest.fixture +def control(monkeypatch): + rclpy.init() + node = manual.ArduSubManualControl() + node.timer.cancel() + clock = SimpleNamespace(seconds=0.0) + monkeypatch.setattr(manual, "time", SimpleNamespace(monotonic=lambda: clock.seconds)) + node.mode_client = Client("custom_mode") + node.arm_client = Client("value") + log = Mock(spec=node.get_logger()) + node.get_logger = lambda: log + yield node, clock, log + node.destroy_node() + rclpy.shutdown() + + +def result(ok=True): + return SimpleNamespace(mode_sent=ok, success=ok) + + +def test_mode_latest_wins_and_log_matches_actual_request(control): + node, _, log = control + node._call_set_mode("STABILIZE") + node._call_set_mode("ALT_HOLD") + node._call_set_mode("MANUAL") + assert node.mode_client.values == ["STABILIZE"] + node.mode_client.futures[0].set_result(result()) + assert node.mode_client.values == ["STABILIZE", "MANUAL"] + log.info.assert_called_with("Requested ArduSub mode: STABILIZE") + node.mode_client.futures[1].set_result(result()) + log.info.assert_called_with("Requested ArduSub mode: MANUAL") + + +def test_selecting_inflight_mode_cancels_other_pending_mode(control): + node, _, _ = control + node._call_set_mode("STABILIZE") + node._call_set_mode("ALT_HOLD") + node._call_set_mode("STABILIZE") + node.mode_client.futures[0].set_result(result()) + assert node.mode_client.values == ["STABILIZE"] + + +def test_disarm_is_preserved_and_arm_is_not_queued(control): + node, _, _ = control + node._call_arm(True) + node._call_arm(False) + node._call_arm(True) + node.arm_client.futures[0].set_result(result()) + assert node.arm_client.values == [True, False] + node.arm_client.futures[1].set_result(result()) + assert node.arm_client.values == [True, False] + + +def test_arm_during_pending_disarm_needs_fresh_press(control): + node, _, _ = control + node._call_arm(False) + node._call_arm(True) + node.arm_client.futures[0].set_result(result()) + assert node.arm_client.values == [False] + node._call_arm(True) + assert node.arm_client.values == [False, True] + + +def test_repeated_disarm_does_not_duplicate_inflight_disarm(control): + node, _, _ = control + node._call_arm(False) + for _ in range(5): + node._call_arm(False) + node.arm_client.futures[0].set_result(result()) + assert node.arm_client.values == [False] + + +@pytest.mark.parametrize("kind", ["mode", "arm"]) +@pytest.mark.parametrize("expire_in_tick", [False, True]) +def test_waiting_mode_expires_but_disarm_is_preserved(control, kind, expire_in_tick): + node, clock, log = control + if kind == "mode": + node._call_set_mode("STABILIZE") + node._call_set_mode("ALT_HOLD") + client = node.mode_client + else: + node._call_arm(True) + node._call_arm(False) + client = node.arm_client + clock.seconds = manual.PENDING_COMMAND_TTL_SEC + 0.01 + if expire_in_tick: + node.tick() + client.futures[0].set_result(result()) + if kind == "mode": + assert client.values == ["STABILIZE"] + assert any("expired" in str(call) for call in log.warning.call_args_list) + else: + assert client.values == [True, False] + assert any("still pending" in str(call) for call in log.warning.call_args_list) + + +@pytest.mark.parametrize("kind", ["mode", "arm"]) +@pytest.mark.parametrize("failure", ["reject", "exception"]) +def test_failed_request_is_not_retried_but_fresh_pending_intent_is_sent(control, kind, failure): + node, _, log = control + if kind == "mode": + node._call_set_mode("STABILIZE") + node._call_set_mode("ALT_HOLD") + client = node.mode_client + expected = ["STABILIZE", "ALT_HOLD"] + else: + node._call_arm(True) + node._call_arm(False) + client = node.arm_client + expected = [True, False] + if failure == "reject": + client.futures[0].set_result(result(False)) + else: + client.futures[0].set_exception(RuntimeError("fixture failure")) + assert client.values == expected + assert log.warning.called + client.futures[1].set_result(result()) + assert client.values == expected + + +@pytest.mark.parametrize("kind", ["mode", "arm"]) +def test_unavailable_service_is_not_queued_for_later_execution(control, kind): + node, _, log = control + client = node.mode_client if kind == "mode" else node.arm_client + call = ( + (lambda: node._call_set_mode("ALT_HOLD")) + if kind == "mode" + else lambda: node._call_arm(True) + ) + client.ready = False + call() + client.ready = True + node.tick() + assert client.values == [] + call() + assert len(client.values) == 1 + assert log.warning.called + + +@pytest.mark.parametrize("kind", ["mode", "arm"]) +def test_synchronous_send_failure_does_not_wedge_client(control, kind): + node, _, _ = control + client = node.mode_client if kind == "mode" else node.arm_client + call = ( + (lambda: node._call_set_mode("ALT_HOLD")) + if kind == "mode" + else lambda: node._call_arm(False) + ) + client.send_error = RuntimeError("send failed") + call() + client.send_error = None + call() + assert len(client.values) == 1 + + +def test_no_timeout_retry_when_service_never_responds(control): + node, clock, log = control + node._call_arm(True) + node._call_arm(False) + node._call_set_mode("STABILIZE") + node._call_set_mode("ALT_HOLD") + clock.seconds = 10.0 + for _ in range(20): + node.tick() + assert node.arm_client.values == [True] + assert node.mode_client.values == ["STABILIZE"] + assert node._pending_mode is None + assert node._pending_disarm is True + assert log.warning.call_count == 2 + + +def test_very_late_arm_response_is_still_followed_by_disarm(control): + node, clock, _ = control + node._call_arm(True) + node._call_arm(False) + clock.seconds = 60.0 + node.tick() + node._call_arm(True) + node.arm_client.futures[0].set_result(result()) + assert node.arm_client.values == [True, False] + node.arm_client.futures[1].set_result(result()) + assert node.arm_client.values == [True, False] diff --git a/models/posim_robot_models/test/test_ws_lifecycle.py b/models/posim_robot_models/test/test_ws_lifecycle.py new file mode 100644 index 00000000..480ce1f7 --- /dev/null +++ b/models/posim_robot_models/test/test_ws_lifecycle.py @@ -0,0 +1,150 @@ +"""Websocket/ROS lifecycle regressions; no simulator or physical hardware.""" + +import asyncio +import importlib.util +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest +from websockets.exceptions import ConnectionClosedError + +spec = importlib.util.spec_from_file_location( + "ws_lifecycle_under_test", Path(__file__).parents[1] / "scripts/ws_to_joy.py" +) +server = importlib.util.module_from_spec(spec) +spec.loader.exec_module(server) + + +class Messages: + def __init__(self, messages, failure=None): + self.messages = iter(messages) + self.failure = failure + + def __aiter__(self): + return self + + async def __anext__(self): + try: + return next(self.messages) + except StopIteration: + if self.failure is not None: + raise self.failure + raise StopAsyncIteration + + +@pytest.fixture +def node(monkeypatch): + # Only supported logger methods are exposed: warn() must never be used. + log = Mock(spec=["info", "warning", "error"]) + n = SimpleNamespace(get_logger=lambda: log, publish_payload=Mock()) + monkeypatch.setattr(server.rclpy, "ok", lambda: True) + return n, log + + +def run(messages, node, stopped=False): + async def body(): + stopping = asyncio.Event() + if stopped: + stopping.set() + await server.relay_messages(messages, node, stopping) + + asyncio.run(body()) + + +def test_valid_payload_is_unchanged(node): + n, log = node + run(Messages(['{"axes":[0,0.5],"buttons":[0,1]}']), n) + n.publish_payload.assert_called_once_with({"axes": [0, 0.5], "buttons": [0, 1]}) + log.error.assert_not_called() + + +def test_bad_json_warns_and_next_input_survives(node): + n, log = node + run(Messages(["not-json", '{"axes":[0]}']), n) + log.warning.assert_called_once() + n.publish_payload.assert_called_once_with({"axes": [0]}) + + +def test_stopped_server_drops_buffered_messages(node): + n, _ = node + run(Messages(["{}"] * 100), n, stopped=True) + n.publish_payload.assert_not_called() + + +def test_invalid_context_drops_buffered_messages(node, monkeypatch): + n, log = node + monkeypatch.setattr(server.rclpy, "ok", lambda: False) + run(Messages(["{}"] * 100), n) + n.publish_payload.assert_not_called() + log.error.assert_not_called() + + +def test_signal_callback_can_run_during_backlog(node): + n, _ = node + + async def body(): + stopping = asyncio.Event() + asyncio.get_running_loop().call_soon(stopping.set) + await server.relay_messages(Messages(["{}"] * 10000), n, stopping) + + asyncio.run(body()) + n.publish_payload.assert_not_called() + + +def test_live_publish_error_is_not_silenced(node): + n, log = node + n.publish_payload.side_effect = [ValueError("bad axes"), None] + run(Messages(["{}", '{"axes":[]}']), n) + assert n.publish_payload.call_count == 2 + log.error.assert_called_once() + + +def test_context_shutdown_race_does_not_log_or_continue(node, monkeypatch): + n, log = node + ok = [True] + monkeypatch.setattr(server.rclpy, "ok", lambda: ok[0]) + + def publish(_): + ok[0] = False + raise RuntimeError("publisher context invalid") + + n.publish_payload.side_effect = publish + run(Messages(["{}"] * 100), n) + n.publish_payload.assert_called_once() + log.error.assert_not_called() + + +def test_dropped_connection_is_expected(node): + n, log = node + run(Messages([], ConnectionClosedError(None, None)), n) + log.error.assert_not_called() + + +def test_bind_failure_cleans_node_and_context(monkeypatch): + events = [] + n = SimpleNamespace( + ws_host="127.0.0.1", ws_port=8765, destroy_node=lambda: events.append("destroy") + ) + monkeypatch.setattr(server.rclpy, "init", lambda **kw: events.append(kw)) + monkeypatch.setattr(server.rclpy, "try_shutdown", lambda: events.append("shutdown")) + monkeypatch.setattr(server, "JoyWebSocketServer", lambda: n) + + def fail(*args, **kwargs): + raise OSError("port busy") + + monkeypatch.setattr(server.websockets, "serve", fail) + with pytest.raises(OSError, match="port busy"): + asyncio.run(server.main_async()) + assert events[0]["signal_handler_options"] == server.SignalHandlerOptions.NO + assert events[-2:] == ["destroy", "shutdown"] + + +def test_node_creation_failure_cleans_context(monkeypatch): + shutdown = Mock() + monkeypatch.setattr(server.rclpy, "init", Mock()) + monkeypatch.setattr(server.rclpy, "try_shutdown", shutdown) + monkeypatch.setattr(server, "JoyWebSocketServer", Mock(side_effect=RuntimeError("node fail"))) + with pytest.raises(RuntimeError, match="node fail"): + asyncio.run(server.main_async()) + shutdown.assert_called_once()