Merge branch 'jogging' into 'v3'

Generalised Jogging

Closes #680

See merge request openflexure/openflexure-microscope-server!476

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,45 @@ class RedefinedBaseMovementError(RuntimeError):
     """
 
 
+class JogCommand:
+    """A base class for jog operations."""
+
+    def __init__(self, displacement: Optional[Sequence[int]]) -> None:
+        """Initialise a JogCommand.
+
+        :param displacement: The distances as a sequence of moves for each axis.
+            None for stop motion.
+        """
+        super().__init__()
+        self.displacement = None if displacement is None else tuple(displacement)
+
+    def __repr__(self) -> str:
+        """Represent the command as a string."""
+        class_name = type(self).__name__
+        if self.displacement is None:
+            return f"<{class_name}>STOP"
+        return f"<{class_name}>{self.displacement}"
+
+
+class JogQueue(queue.Queue[JogCommand]):
+    """A class queue for JogCommands. This always returns the most recent command."""
+
+    def __init__(self) -> None:
+        """Set up a queue with a max size of 1."""
+        super().__init__(maxsize=1)
+
+    def put(
+        self, item: JogCommand, block: bool = False, timeout: Optional[float] = None
+    ) -> None:
+        """Put the next command into the queue, bumping anything already there."""
+        try:
+            # First remove existing item if present
+            self.get_nowait()
+        except queue.Empty:
+            pass
+        super().put(item, block=block, timeout=timeout)
+
+
 class BaseStage(lt.Thing):
     """A base stage class for OpenFlexure translation stages.
 
@@ -55,7 +96,11 @@ class BaseStage(lt.Thing):
             that all code in the child class uses the hardware reference frame.
         """
         super().__init__(thing_server_interface)
-        self._hardware_position = dict.fromkeys(self._axis_names, 0)
+        self._hardware_lock = threading.RLock()
+        self._jog_lock = threading.Lock()
+        self._jog_queue = JogQueue()
+        self._jog_thread: Optional[threading.Thread] = None
+        self._hardware_position: Mapping[str, int] = dict.fromkeys(self._axis_names, 0)
 
         # This must be the last thing the function does in case it is caught in a try.
         if (
@@ -87,6 +132,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]: ...
 
@@ -177,9 +228,129 @@ class BaseStage(lt.Thing):
         Make sure to use and update ``self._hardware_position`` not ``self.position``.
         """
         raise NotImplementedError(
-            "StageThings must define their own move_absolute method"
+            "StageThings must define their own _hardware_move_absolute method"
         )
 
+    def _hardware_start_move_relative(self, displacement: Sequence[int]) -> None:
+        """Start a relative move."""
+        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."""
+        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."""
+        raise NotImplementedError(
+            "StageThings must define their own _estimate_move_duration method"
+        )
+
+    @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(JogCommand(None))
+            return
+
+        hardware_moves = self._apply_axis_direction(kwargs)
+        move = [hardware_moves.get(axis, 0) for axis in self.axis_names]
+        if all(ax == 0 for ax in move):
+            self.logger.warning(
+                "Requested jog movement is is empty. Sending STOP instead."
+            )
+            self._send_jog_command(JogCommand(None))
+        else:
+            self._send_jog_command(JogCommand(move))
+
+    def _send_jog_command(self, command: JogCommand) -> 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.
+        """
+        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.debug("Starting background thread for jog commands")
+                self._jog_queue = JogQueue()
+                self._jog_thread = threading.Thread(
+                    target=self._jog_loop, args=(command,)
+                )
+                self._jog_thread.start()
+            else:
+                self._jog_queue.put(command)
+        finally:
+            self._jog_lock.release()
+
+    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.
+        """
+        # Timeout for checking queue
+        timeout = 0.1
+        command: Optional[JogCommand] = first_command
+
+        # prevent others using the stage while jogging.
+        with self._hardware_lock:
+            while command is not None:
+                if command.displacement is not None:
+                    self._hardware_start_move_relative(command.displacement)
+                    timeout = self._estimate_move_duration(command.displacement)
+                else:
+                    self._hardware_stop()
+                    # Next iteration, we will probably time out.
+                    timeout = 0.1
+                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.

src/openflexure_microscope_server/things/stage/dummy.py

@@ -2,15 +2,30 @@
 
 from __future__ import annotations
 
+import queue
+import threading
 import time
+from collections.abc import Sequence
 from types import TracebackType
-from typing import Any, Optional, Self
+from typing import Any, Mapping, Optional, Self
 
 import labthings_fastapi as lt
 
 from . import BaseStage
 
 
+class DummyStageMovement:
+    """A class used internally to send movements to the dummy stages movement thread."""
+
+    def __init__(self, displacement: Optional[Sequence[int]]) -> None:
+        """Initialise with a displacement.
+
+        :param displacement: A sequence of integers or None to stop the motion.
+        """
+        self.started = threading.Event()
+        self.displacement = displacement
+
+
 class DummyStage(BaseStage):
     """A dummy stage for testing purposes.
 
@@ -32,12 +47,22 @@ class DummyStage(BaseStage):
             also doing computationally heavy tasks like simulated image blurring.
         """
         super().__init__(thing_server_interface, **kwargs)
+        self._movement_enabled = False
+        self._move_thread: Optional[threading.Thread] = None
+        self._move_queue = queue.Queue[DummyStageMovement]()
+        self._movement_ongoing = False
+
         self.step_time = step_time
-        self.instantaneous_position = self._hardware_position
+        self.instantaneous_position: Mapping[str, int] = self._hardware_position
+        self._inst_pos_lock = threading.Lock()
+        self._abort_move = threading.Event()
 
     def __enter__(self) -> Self:
-        """Register the stage position when the Thing context manager is opened."""
+        """Register the stage position and start move thread running."""
         self.instantaneous_position = self._hardware_position
+        self._movement_enabled = True
+        self._move_thread = threading.Thread(target=self._move_loop)
+        self._move_thread.start()
         return self
 
     def __exit__(
@@ -46,49 +71,143 @@ class DummyStage(BaseStage):
         _exc_value: Optional[BaseException],
         _traceback: Optional[TracebackType],
     ) -> None:
-        """Nothing to do when the Thing context manager is closed."""
+        """Stop the move thread."""
+        if self._move_thread is not None and self._move_thread.is_alive():
+            self._movement_enabled = False
+            self._move_thread.join()
 
     axis_inverted: dict[str, bool] = lt.setting(
         default={"x": True, "y": False, "z": False}, readonly=True
     )
     """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."""
+        pass
+
+    def _set_pos_during_move(
+        self, displacement: Sequence[int], fraction_complete: float
+    ) -> None:
+        """Set the instantaneous position based on the completed fraction of an ongoing move."""
+        self.instantaneous_position = {
+            ax: self._hardware_position[ax] + int(fraction_complete * disp)
+            for ax, disp in zip(self.axis_names, displacement, strict=True)
+        }
+
+    def _move_loop(self) -> None:
+        """Run the move loop. This should be run in a thread on enter.
+
+        This controls all movement of the dummy stage.
+        """
+        # Start with no move request
+        movement_request: Optional[DummyStageMovement] = None
+        # Loop until the server ends.
+        while self._movement_enabled:
+            # First check for a new movement request. These will interrupt ongoing moves
+            movement_request = self._check_for_new_move_request()
+
+            # If there is a new movement.
+            if movement_request is not None:
+                # Set the hardware position from instantaneous before continuing.
+                self._hardware_position = self.instantaneous_position
+
+                if movement_request.displacement is None:
+                    # If it is a stop command, stop moving
+                    self._movement_ongoing = False
+                    movement_request.started.set()
+                else:
+                    # Record the displacement for future iterations
+                    displacement = movement_request.displacement
+                    # If it is a move command, set up the variables for the move
+                    self._movement_ongoing = True
+                    movement_request.started.set()
+                    fraction_complete = 0.0
+                    dt = self.step_time
+                    max_displacement = max(abs(v) for v in displacement)
+                    # Always wait at least dt
+                    total_time = max((dt * max_displacement), dt)
+                    start_time = time.time()
+                    time.sleep(dt)
+
+            # If a movement is ongoing
+            if self._movement_ongoing:
+                fraction_complete = (time.time() - start_time) / total_time
+                if fraction_complete < 1:
+                    self._set_pos_during_move(displacement, fraction_complete)
+                else:
+                    # move is complete
+                    fraction_complete = 1.0
+                    self._set_pos_during_move(displacement, fraction_complete)
+                    self._hardware_position = self.instantaneous_position
+                    self._movement_ongoing = False
+
+    def _check_for_new_move_request(self) -> Optional[DummyStageMovement]:
+        """Check for new move request to the move_loop."""
+        try:
+            # Timeout sets the motor speed time
+            timeout = self.step_time * 10 if self._movement_ongoing else 0.1
+            return self._move_queue.get(timeout=timeout)
+        except queue.Empty:
+            return None
+
+    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.
+        """
+        with self._hardware_lock:
+            self.moving = True
+            self._move_queue.put(DummyStageMovement(displacement))
+
+    def _hardware_stop(self) -> None:
+        with self._hardware_lock:
+            self._move_queue.put(DummyStageMovement(None))
+            self.moving = False
+
+    def _poll_moving(self) -> bool:
+        """Determine if the stage is still moving."""
+        moving = self._movement_ongoing
+        if self.moving != moving:
+            self.moving = moving
+        return moving
+
+    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)
+        # This does not yet check the board's speed.
+        return max_displacement * self.step_time
+
     def _hardware_move_relative(
         self,
         block_cancellation: bool = False,
         **kwargs: int,
     ) -> None:
         """Make a relative move. Keyword arguments should be axis names."""
-        displacement = [kwargs.get(k, 0) for k in self.axis_names]
-        self.moving = True
-        try:
-            fraction_complete = 0.0
-            dt = self.step_time
-            max_displacement = max(abs(v) for v in displacement)
-            start_time = time.time()
-            while time.time() - start_time < dt * max_displacement:
-                if block_cancellation:
-                    time.sleep(dt)
-                else:
-                    lt.cancellable_sleep(dt)
-                fraction_complete = (time.time() - start_time) / (dt * max_displacement)
-                self.instantaneous_position = {
-                    ax: self._hardware_position[ax] + int(fraction_complete * disp)
-                    for ax, disp in zip(self.axis_names, displacement, strict=True)
-                }
-            fraction_complete = 1.0
-        except lt.exceptions.InvocationCancelledError as e:
-            # If the move has been cancelled, stop it but don't handle the exception.
-            # We need the exception to propagate in order to stop any calling tasks,
-            # and to mark the invocation as "cancelled" rather than stopped.
-            raise e
-        finally:
-            self.moving = False
-            self._hardware_position = {
-                ax: self._hardware_position[ax] + int(fraction_complete * disp)
-                for ax, disp in zip(self.axis_names, displacement, strict=True)
-            }
-            self.instantaneous_position = self._hardware_position
+        with self._hardware_lock:
+            displacement = [kwargs.get(k, 0) for k in self.axis_names]
+            self.moving = True
+            move_request = DummyStageMovement(displacement)
+            self._move_queue.put(move_request)
+
+            move_request.started.wait(timeout=0.1)
+            while self._movement_ongoing:
+                try:
+                    if block_cancellation:
+                        time.sleep(self.step_time * 10)
+                    else:
+                        lt.cancellable_sleep(self.step_time * 10)
+
+                except lt.exceptions.InvocationCancelledError as e:
+                    # If the move has been cancelled, stop it but don't handle the
+                    # exception. We need the exception to propagate in order to stop
+                    # any calling tasks, and to mark the invocation as "cancelled"
+                    # rather than stopped.
+                    self._move_queue.put(DummyStageMovement(None))
+                    raise e
+                finally:
+                    self.moving = False
 
     def _hardware_move_absolute(
         self,
@@ -113,5 +232,6 @@ class DummyStage(BaseStage):
         It is intended for use after manually or automatically recentring the
         stage.
         """
-        self._hardware_position = dict.fromkeys(self.axis_names, 0)
-        self.instantaneous_position = self._hardware_position
+        with self._hardware_lock:
+            self._hardware_position = dict.fromkeys(self.axis_names, 0)
+            self.instantaneous_position = self._hardware_position

src/openflexure_microscope_server/things/stage/sangaboard.py

@@ -2,11 +2,10 @@
 
 from __future__ import annotations
 
-import threading
-from contextlib import contextmanager
+from collections.abc import Sequence
 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
 
@@ -49,16 +48,21 @@ class SangaboardThing(BaseStage):
         """
         self.sangaboard_kwargs = copy(kwargs)
         self.sangaboard_kwargs["port"] = port
-        self._sangaboard_lock = threading.RLock()
+        # Set a default step time, before reading on enter.
+        self._step_time = 0.001
+
         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")
+            # Read step time only on enter as there is no provision for changing speed.
+            self._step_time = self._sangaboard.step_time * 1e-6
         self.check_firmware()
         self.update_position()
+
         return self
 
     def __exit__(
@@ -68,17 +72,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
@@ -87,9 +82,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:
@@ -99,11 +94,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:
@@ -119,6 +114,41 @@ class SangaboardThing(BaseStage):
                     f"{RECOMMENDED_VERSION}. Visit {WIKI_URL} for instructions on updating."
                 )
 
+    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.
+        """
+        with self._hardware_lock:
+            self.moving = True
+            self._sangaboard.move_rel(displacement)
+
+    def _hardware_stop(self) -> None:
+        """Stop any motion of the stage as soon as possible."""
+        with self._hardware_lock:
+            self._sangaboard.query("stop")
+            self.moving = False
+
+    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.
+        """
+        with self._hardware_lock:
+            moving = self._sangaboard.query("moving?") == "true"
+            if self.moving != moving:
+                self.moving = moving
+            return moving
+
+    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)
+        # This does not yet check the board's speed.
+        return max_displacement * self._step_time
+
     def _hardware_move_relative(
         self,
         block_cancellation: bool = False,
@@ -126,20 +156,24 @@ class SangaboardThing(BaseStage):
     ) -> None:
         """Make a relative move in the coordinate system used by the sangaboard."""
         displacement = [kwargs.get(axis, 0) for axis in self.axis_names]
-        with self.sangaboard() as sb:
-            self.moving = True
+        duration = self._estimate_move_duration(displacement)
+        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:
-                    while sb.query("moving?") == "true":
+                    if duration > 0.2:
+                        lt.cancellable_sleep(
+                            duration - 0.1
+                        )  # Avoid unnecessary polling
+                    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.
                 # We need the exception to propagate in order to stop any calling tasks,
                 # and to mark the invocation as "cancelled" rather than stopped.
-                sb.query("stop")
+                self._hardware_stop()
                 raise e
             finally:
                 self.moving = False
@@ -151,7 +185,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]
@@ -170,8 +204,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(
@@ -186,8 +220,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")
 
@@ -196,6 +230,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")
+                self._sangaboard.query(f"{led_command} 0")