From 70dd7bd99420ea1ca0ee4668a911e5c69562188e Mon Sep 17 00:00:00 2001 From: Julian Stirling Date: Thu, 12 Feb 2026 20:57:04 +0000 Subject: [PATCH] Generalise jog --- .../things/stage/__init__.py | 208 ++++++++++++++- .../things/stage/sangaboard.py | 238 ++---------------- 2 files changed, 234 insertions(+), 212 deletions(-) diff --git a/src/openflexure_microscope_server/things/stage/__init__.py b/src/openflexure_microscope_server/things/stage/__init__.py index fd495186..c6f2fb78 100644 --- a/src/openflexure_microscope_server/things/stage/__init__.py +++ b/src/openflexure_microscope_server/things/stage/__init__.py @@ -11,8 +11,10 @@ As the object will be used as a context manager create the hardware connection i from __future__ import annotations +import queue +import threading from collections.abc import Mapping, Sequence -from typing import Any, Literal, overload +from typing import Any, Literal, Optional, overload import labthings_fastapi as lt @@ -30,6 +32,40 @@ class RedefinedBaseMovementError(RuntimeError): """ +class JogCommand: + """A base class for jog operations.""" + + def __init__(self) -> None: + """Initialise a JogCommand.""" + super().__init__() + self.received = threading.Event() + self.finished = threading.Event() + + def __repr__(self) -> str: + """Represent the command as a string.""" + return f"<{self.__class__.__name__}>" + + +class JogMoveCommand(JogCommand): + """A command to make a jog move.""" + + def __init__(self, displacement: Sequence[int]) -> None: + """Initialise a JogMoveCommand. + + :param displacement: The displacement for the jog move, in hardware coordinates. + """ + super().__init__() + self.displacement = displacement + + def __repr__(self) -> str: + """Represent the command as a string.""" + return f"<{self.__class__.__name__} {self.displacement}>" + + +class JogStopCommand(JogCommand): + """A command to stop a jog move.""" + + class BaseStage(lt.Thing): """A base stage class for OpenFlexure translation stages. @@ -55,6 +91,10 @@ class BaseStage(lt.Thing): that all code in the child class uses the hardware reference frame. """ super().__init__(thing_server_interface) + self._hardware_lock = threading.RLock() + self._jog_lock = threading.Lock() + self._jog_queue: queue.Queue[JogCommand] = queue.Queue[JogCommand](maxsize=1) + self._jog_thread: Optional[threading.Thread] = None self._hardware_position = dict.fromkeys(self._axis_names, 0) # This must be the last thing the function does in case it is caught in a try. @@ -87,6 +127,12 @@ class BaseStage(lt.Thing): ) """Used to convert coordinates between the program frame and the hardware frame.""" + def update_position(self) -> None: + """Read position from the stage and set the corresponding property.""" + raise NotImplementedError( + "StageThings must define their own update_position method" + ) + @overload def _apply_axis_direction(self, position: list[int] | tuple[int]) -> list[int]: ... @@ -180,6 +226,166 @@ class BaseStage(lt.Thing): "StageThings must define their own move_absolute method" ) + def _hardware_start_move_relative(self, displacement: Sequence[int]) -> None: + """Start a relative move. + + This starts the stage moving, but does not wait for the move to complete. It + sets ``self.moving`` to ``True``: resetting it is the responsibility of the + calling code. + """ + raise NotImplementedError( + "StageThings must define their own hardware_start_move_relative method" + ) + + def _hardware_stop(self) -> None: + raise NotImplementedError( + "StageThings must define their own _hardware_stop method" + ) + + def _poll_moving(self) -> bool: + """Determine if the stage is still moving. + + This also sets ``moving`` if the status has changed. + + :return: whether the stage is still moving. + """ + raise NotImplementedError( + "StageThings must define their own _poll_moving method" + ) + + def _estimate_move_duration(self, displacement: Sequence[int]) -> float: + """Calculate the expected duration of a move with the given displacement.""" + max_displacement = max(abs(d) for d in displacement) + return max_displacement * 0.001 # This does not yet check the board's speed. + + @lt.action + def jog(self, stop: bool = False, **kwargs: int) -> None: + """Make a relative move that may be interrupted by a future ``jog``. + + This action makes a relative move. If another ``jog`` action is called while + a ``jog`` is already in progress, the first will be stopped and the second + will start immediately. This allows for responsive manual control of the + stage, for example with a joystick. + + :param stop: if this is set to ``True`` the jog will be terminated. + :param kwargs: Keyword arguments should be axis names. + """ + if stop: + self._send_jog_command(JogStopCommand(), timeout=1) + else: + self._hardware_jog(**self._apply_axis_direction(kwargs)) + + def _hardware_jog(self, **kwargs: int) -> None: + """Make a relative move that may be interrupted by a future ``jog``. + + This function uses hardware coordinates, ``jog`` is the public wrapper that + applies any neecessary transform and then calls this function. See the + docs for that function for more explanation. + + See `_jog_loop` for an explanation of the mechanism. + + :param kwargs: Keyword arguments should be axis names. + """ + move = [kwargs.get(axis, 0) for axis in self.axis_names] + self._send_jog_command( + JogMoveCommand(move), + timeout=self._estimate_move_duration(move) + 1, + ) + + def _send_jog_command( + self, command: JogCommand, timeout: Optional[float] = None + ) -> None: + """Send a jog command to the background jog thread. + + This function will start the background thread if it is not running. + This function acquires ``_jog_lock`` and uses the ``_jog_send`` event to signal + the thread to read the next command. As commands interrupt each other, this + function should never block for a long time. + + :param command: the jog command to send. + :param timeout: how long to wait for the command to be completed, or ``None`` + to skip waiting. + """ + if not self._jog_lock.acquire(timeout=0.1): + self.logger.warning( + "Could not send a jog message, this indicates a lock error." + ) + return + try: + # Make sure the queue exists. + # Check the background thread is running, and restart it if not. + if self._jog_thread is None or not self._jog_thread.is_alive(): + self.logger.info("Starting background thread for jog commands") + self._jog_queue = queue.Queue[JogCommand](maxsize=1) + self._jog_thread = threading.Thread( + target=self._jog_loop, args=(command,) + ) + self._jog_thread.start() + else: + self._jog_queue.put(command) + command.received.wait(1) + finally: + self._jog_lock.release() + # The final wait happens after releasing the lock: this allows jog moves to + # be interrupted. + if timeout is not None: + command.finished.wait(timeout) + + def _jog_loop(self, first_command: JogCommand) -> None: + """Execute jog commands in a background thread. + + This function is intended to be run in a background thread. It will look at + ``self._jog_command`` when the ``self._jog_send`` event is set, and signal that + the command is being processed with ``self._jog_received``. + + If no new command is received before the current command finishes, the thread + will terminate, and ``self._jog_received`` will be set again. This means that + it should be safe to + """ + # Timeout for checking queue + timeout = 0.1 + previous_command: Optional[JogCommand] = None + command: Optional[JogCommand] = first_command + + # prevent others using the stage while jogging. + with self._hardware_lock: + while command is not None: + # Acknowledge the command + command.received.set() + if previous_command: + # Notify the last command it's superseded + previous_command.finished.set() + previous_command = command + if isinstance(command, JogMoveCommand): + self._hardware_start_move_relative(command.displacement) + timeout = self._estimate_move_duration(command.displacement) + elif isinstance(command, JogStopCommand): + self._hardware_stop() + # Next iteration, we will probably time out. + timeout = 0.1 + else: + raise RuntimeError(f"Unknown jog command: {command}") + self.update_position() + command = self._get_from_jog_queue(timeout) + + def _get_from_jog_queue(self, timeout: float) -> Optional[JogCommand]: + """Get the next JogCommand from the jog queue. + + :param timeout: The estimtated time the move will take for the queue timeout. + :return: The jog command or None if the stage stops before a command is + received. + """ + while True: + try: + return self._jog_queue.get(timeout=timeout) + except queue.Empty: + if not self._poll_moving(): + # The stage is no longer moving, return None + return None + # If we reached here then the stage is still moving shorten timeout and + # check again. + timeout = 0.1 + @lt.action def set_zero_position(self) -> None: """Make the current position zero in all axes. diff --git a/src/openflexure_microscope_server/things/stage/sangaboard.py b/src/openflexure_microscope_server/things/stage/sangaboard.py index 8e3ae14d..5bf69420 100644 --- a/src/openflexure_microscope_server/things/stage/sangaboard.py +++ b/src/openflexure_microscope_server/things/stage/sangaboard.py @@ -2,13 +2,10 @@ from __future__ import annotations -import queue -import threading from collections.abc import Sequence -from contextlib import contextmanager from copy import copy from types import TracebackType -from typing import Any, Iterator, Literal, Optional, Self +from typing import Any, Literal, Optional, Self import semver @@ -21,40 +18,6 @@ REQUIRED_VERSION = semver.Version.parse("1.0.0") RECOMMENDED_VERSION = semver.Version.parse("1.0.4") -class JogCommand: - """A base class for jog operations.""" - - def __init__(self) -> None: - """Initialise a JogCommand.""" - super().__init__() - self.received = threading.Event() - self.finished = threading.Event() - - def __repr__(self) -> str: - """Represent the command as a string.""" - return f"<{self.__class__.__name__}>" - - -class JogMoveCommand(JogCommand): - """A command to make a jog move.""" - - def __init__(self, displacement: Sequence[int]) -> None: - """Initialise a JogMoveCommand. - - :param displacement: The displacement for the jog move, in hardware coordinates. - """ - super().__init__() - self.displacement = displacement - - def __repr__(self) -> str: - """Represent the command as a string.""" - return f"<{self.__class__.__name__} {self.displacement}>" - - -class JogStopCommand(JogCommand): - """A command to stop a jog move.""" - - class SangaboardThing(BaseStage): """A Thing to manage a Sangaboard motor controller. @@ -84,19 +47,14 @@ class SangaboardThing(BaseStage): """ self.sangaboard_kwargs = copy(kwargs) self.sangaboard_kwargs["port"] = port - self._sangaboard_lock = threading.RLock() - - self._jog_lock = threading.Lock() - self._jog_queue: queue.Queue[JogCommand] = queue.Queue[JogCommand](maxsize=1) - self._jog_thread: Optional[threading.Thread] = None super().__init__(thing_server_interface, **kwargs) def __enter__(self) -> Self: """Connect to the sangaboard when the Thing context manager is opened.""" self._sangaboard = sangaboard.Sangaboard(**self.sangaboard_kwargs) - with self.sangaboard() as sb: - sb.query("blocking_moves false") + with self._hardware_lock: + self._sangaboard.query("blocking_moves false") self.check_firmware() self.update_position() return self @@ -108,17 +66,8 @@ class SangaboardThing(BaseStage): _traceback: Optional[TracebackType], ) -> None: """Close the sangaboard connection when the Thing context manager is closed.""" - with self.sangaboard() as sb: - sb.close() - - @contextmanager - def sangaboard(self) -> Iterator[sangaboard.Sangaboard]: - """Return the wrapped ``sangaboard.Sangaboard`` instance. - - This is protected by a ``threading.RLock``, which may change in future. - """ - with self._sangaboard_lock: - yield self._sangaboard + with self._hardware_lock: + self._sangaboard.close() axis_inverted: dict[str, bool] = lt.setting( default={"x": True, "y": False, "z": True}, readonly=True @@ -127,9 +76,9 @@ class SangaboardThing(BaseStage): def update_position(self) -> None: """Read position from the stage and set the corresponding property.""" - with self.sangaboard() as sb: + with self._hardware_lock: self._hardware_position = dict( - zip(self.axis_names, sb.position, strict=True) + zip(self.axis_names, self._sangaboard.position, strict=True) ) def check_firmware(self) -> None: @@ -139,11 +88,11 @@ class SangaboardThing(BaseStage): Log a warning if the version is below RECOMMENDED_VERSION """ - with self.sangaboard() as sb: + with self._hardware_lock: # This will raise a ValueError is if the firmware version cannot be parsed # this error will stop the microscope booting as we cannot ensure valid # firmware. - version = semver.Version.parse(sb.firmware_version) + version = semver.Version.parse(self._sangaboard.firmware_version) # Raise an error if version is below required if version < REQUIRED_VERSION: @@ -165,21 +114,16 @@ class SangaboardThing(BaseStage): This starts the stage moving, but does not wait for the move to complete. It sets ``self.moving`` to ``True``: resetting it is the responsibility of the calling code. """ - with self.sangaboard() as sb: + with self._hardware_lock: self.moving = True - sb.move_rel(displacement) + self._sangaboard.move_rel(displacement) def _hardware_stop(self) -> None: """Stop any motion of the stage as soon as possible.""" - with self.sangaboard() as sb: - sb.query("stop") + with self._hardware_lock: + self._sangaboard.query("stop") self.moving = False - def _estimate_move_duration(self, displacement: Sequence[int]) -> float: - """Calculate the expected duration of a move with the given displacement.""" - max_displacement = max(abs(d) for d in displacement) - return max_displacement * 0.001 # This does not yet check the board's speed. - def _poll_moving(self) -> bool: """Determine if the stage is still moving. @@ -187,15 +131,15 @@ class SangaboardThing(BaseStage): :return: whether the stage is still moving. """ - with self.sangaboard() as sb: - moving = sb.query("moving?") == "true" + with self._hardware_lock: + moving = self._sangaboard.query("moving?") == "true" if self.moving != moving: self.moving = moving return moving def _poll_until_stopped(self) -> None: """Poll the stage until it reports that it has stopped moving.""" - with self.sangaboard(): + with self._hardware_lock: while self._poll_moving(): lt.cancellable_sleep(0.1) @@ -207,17 +151,17 @@ class SangaboardThing(BaseStage): """Make a relative move in the coordinate system used by the sangaboard.""" displacement = [kwargs.get(axis, 0) for axis in self.axis_names] duration = self._estimate_move_duration(displacement) - with self.sangaboard() as sb: + with self._hardware_lock: try: - sb.move_rel(displacement) + self._sangaboard.move_rel(displacement) if block_cancellation: - sb.query("notify_on_stop") + self._sangaboard.query("notify_on_stop") else: if duration > 0.2: lt.cancellable_sleep( duration - 0.1 ) # Avoid unnecessary polling - while sb.query("moving?") == "true": + while self._sangaboard.query("moving?") == "true": lt.cancellable_sleep(0.1) except lt.exceptions.InvocationCancelledError as e: # If the move has been cancelled, stop it but don't handle the exception. @@ -235,7 +179,7 @@ class SangaboardThing(BaseStage): **kwargs: int, ) -> None: """Make a absolute move in the coordinate system used by the sangaboard.""" - with self.sangaboard(): + with self._hardware_lock: self.update_position() displacement = { axis: int(pos) - self._hardware_position[axis] @@ -254,8 +198,8 @@ class SangaboardThing(BaseStage): It is intended for use after manually or automatically recentring the stage. """ - with self.sangaboard() as sb: - sb.zero_position() + with self._hardware_lock: + self._sangaboard.zero_position() self.update_position() def set_led( @@ -270,8 +214,8 @@ class SangaboardThing(BaseStage): being addressed. """ led_command = f"led_{led_channel}" - with self.sangaboard() as sb: - return_value = sb.query(f"{led_command}?") + with self._hardware_lock: + return_value = self._sangaboard.query(f"{led_command}?") if not return_value.startswith("CC LED:"): raise IOError("The sangaboard does not support LED control") @@ -280,134 +224,6 @@ class SangaboardThing(BaseStage): # cannot be used. if led_on: on_brightness = 0.32 - sb.query(f"{led_command} {on_brightness}") + self._sangaboard.query(f"{led_command} {on_brightness}") else: - sb.query(f"{led_command} 0") - - @lt.action - def jog(self, stop: bool = False, **kwargs: int) -> None: - """Make a relative move that may be interrupted by a future ``jog``. - - This action makes a relative move. If another ``jog`` action is called while - a ``jog`` is already in progress, the first will be stopped and the second - will start immediately. This allows for responsive manual control of the - stage, for example with a joystick. - - :param stop: if this is set to ``True`` the jog will be terminated. - :param kwargs: Keyword arguments should be axis names. - """ - if stop: - self._send_jog_command(JogStopCommand(), timeout=1) - else: - self._hardware_jog(**self._apply_axis_direction(kwargs)) - - def _hardware_jog(self, **kwargs: int) -> None: - """Make a relative move that may be interrupted by a future ``jog``. - - This function uses hardware coordinates, ``jog`` is the public wrapper that - applies any neecessary transform and then calls this function. See the - docs for that function for more explanation. - - See `_jog_loop` for an explanation of the mechanism. - - :param kwargs: Keyword arguments should be axis names. - """ - move = [kwargs.get(axis, 0) for axis in self.axis_names] - self._send_jog_command( - JogMoveCommand(move), - timeout=self._estimate_move_duration(move) + 1, - ) - - def _send_jog_command( - self, command: JogCommand, timeout: Optional[float] = None - ) -> None: - """Send a jog command to the background jog thread. - - This function will start the background thread if it is not running. - This function acquires ``_jog_lock`` and uses the ``_jog_send`` event to signal - the thread to read the next command. As commands interrupt each other, this - function should never block for a long time. - - :param command: the jog command to send. - :param timeout: how long to wait for the command to be completed, or ``None`` - to skip waiting. - """ - if not self._jog_lock.acquire(timeout=0.1): - self.logger.warning( - "Could not send a jog message, this indicates a lock error." - ) - return - try: - # Make sure the queue exists. - # Check the background thread is running, and restart it if not. - if self._jog_thread is None or not self._jog_thread.is_alive(): - self.logger.info("Starting background thread for jog commands") - self._jog_queue = queue.Queue[JogCommand](maxsize=1) - self._jog_thread = threading.Thread( - target=self._jog_loop, args=(command,) - ) - self._jog_thread.start() - else: - self._jog_queue.put(command) - command.received.wait(1) - finally: - self._jog_lock.release() - # The final wait happens after releasing the lock: this allows jog moves to - # be interrupted. - if timeout is not None: - command.finished.wait(timeout) - - def _jog_loop(self, first_command: JogCommand) -> None: - """Execute jog commands in a background thread. - - This function is intended to be run in a background thread. It will look at - ``self._jog_command`` when the ``self._jog_send`` event is set, and signal that - the command is being processed with ``self._jog_received``. - - If no new command is received before the current command finishes, the thread - will terminate, and ``self._jog_received`` will be set again. This means that - it should be safe to - """ - # Timeout for checking queue - timeout = 0.1 - previous_command: Optional[JogCommand] = None - command: Optional[JogCommand] = first_command - - # prevent others using the sangaboard while jogging. - with self.sangaboard() as _sb: - while command is not None: - # Acknowledge the command - command.received.set() - if previous_command: - # Notify the last command it's superseded - previous_command.finished.set() - previous_command = command - if isinstance(command, JogMoveCommand): - self._hardware_start_move_relative(command.displacement) - timeout = self._estimate_move_duration(command.displacement) - elif isinstance(command, JogStopCommand): - self._hardware_stop() - # Next iteration, we will probably time out. - timeout = 0.1 - else: - raise RuntimeError(f"Unknown jog command: {command}") - self.update_position() - command = self._get_from_jog_queue(timeout) - - def _get_from_jog_queue(self, timeout: int) -> Optional[JogCommand]: - """Get the next JogCommand from the jog queue. - - :param timeout: The estimtated time the move will take for the queue timeout. - :return: The jog command or None if the stage stops before a command is - received. - """ - while True: - try: - return self._jog_queue.get(timeout=timeout) - except queue.Empty: - if not self._poll_moving(): - # The stage is no longer moving, return None - return None - # If we reached here then the stage is still moving shorten timeout and - # check again. - timeout = 0.1 + self._sangaboard.query(f"{led_command} 0")