Source code for genericroboticarm.sila_server.server

# Generated by sila2.code_generator; sila2.__version__: 0.10.4

import logging
from contextlib import contextmanager
from typing import Callable, Iterator, Optional
from uuid import UUID
from threading import BoundedSemaphore, Thread

from sila2.server import SilaServer

from genericroboticarm.robo_APIs import InteractiveTransfer, RoboInterface

from .feature_implementations.implicitinteractionservice_impl import ImplicitInteractionServiceImpl
from .feature_implementations.intermediateactionplanning_impl import IntermediateActionPlanningImpl
from .feature_implementations.labwaretransfermanipulatorcontroller_impl import LabwareTransferManipulatorControllerImpl
from .feature_implementations.robotcontroller_impl import RobotControllerImpl
from .feature_implementations.roboteachingservice_impl import RoboTeachingServiceImpl
from .generated.implicitinteractionservice import ImplicitInteractionServiceFeature
from .generated.intermediateactionplanning import IntermediateActionPlanningFeature
from .generated.labwaretransfermanipulatorcontroller import LabwareTransferManipulatorControllerFeature
from .generated.robotcontroller import RobotControllerFeature
from .generated.roboteachingservice import RoboTeachingServiceFeature
from datetime import datetime




class RobotCommandQueueFull(RuntimeError):
    pass





class DeferredTaskFailed(RuntimeError):
    """
    Raised on the next command when a deferred tail task of a previous command failed.
    The robot is likely in an unexpected state, e.g. still holding a lid.
    """





class Server(SilaServer):
    robot: RoboInterface

    def __init__(self, robo_interface: RoboInterface, server_uuid: Optional[UUID] = None):
        super().__init__(
            server_name=robo_interface.name,
            server_type="RoboArmServer",
            server_version="0.1",
            server_description=f"Server to control a {robo_interface.name}",
            server_vendor_url="https://gitlab.com/SiLA2/sila_python",
            server_uuid=server_uuid,
        )
        self.robot = robo_interface
        # one command executing, plus commands waiting for the robot. A deferred tail task can
        # keep the robot busy without occupying a slot, so there is room for one more waiter.
        self.robot_command_slots = BoundedSemaphore(3)

        self.robotcontroller = RobotControllerImpl(self)
        self.set_feature_implementation(RobotControllerFeature, self.robotcontroller)

        self.roboteachingservice = RoboTeachingServiceImpl(self)
        self.set_feature_implementation(RoboTeachingServiceFeature, self.roboteachingservice)

        print(f"{datetime.now()}: is interactive: ", isinstance(robo_interface, InteractiveTransfer))
        if isinstance(robo_interface, InteractiveTransfer):
            self.labwaretransfermanipulatorcontroller = LabwareTransferManipulatorControllerImpl(self)
            self.set_feature_implementation(
                LabwareTransferManipulatorControllerFeature, self.labwaretransfermanipulatorcontroller
            )
            # lets clients announce the intermediate actions of a transfer to PrepareForInput
            self.intermediateactionplanning = IntermediateActionPlanningImpl(self)
            self.set_feature_implementation(IntermediateActionPlanningFeature, self.intermediateactionplanning)

        self.implicitinteractionservice = ImplicitInteractionServiceImpl(self, robo_interface.interacting_devices)
        self.set_feature_implementation(ImplicitInteractionServiceFeature, self.implicitinteractionservice)



    def stop(self, grace_period: Optional[float] = None) -> None:
        self.robot.manual_mover.stop()
        self.robot.end_connection()
        super().stop(grace_period)




    @contextmanager
    def robot_command(self) -> Iterator[None]:
        accepted = self.robot_command_slots.acquire(blocking=False)
        if not accepted:
            raise RobotCommandQueueFull(
                "Robot is already executing one SiLA command and has commands waiting."
            )
        try:
            # reserve the robot for the whole task, which may outlive this command
            self.robot.task_lock.acquire()
            handed_over = False
            try:
                # a tail task failing after its command reported success can only be reported
                # here. Taking it after acquiring the lock ensures the tail task is finished.
                failure = self.robot.take_deferred_failure()
                if failure is not None:
                    raise DeferredTaskFailed(
                        f"A deferred task of a previous command failed: {failure}"
                    ) from failure
                yield
                tail_task = self.robot.pop_deferred()
                if tail_task is not None:
                    self._run_deferred(tail_task)
                    handed_over = True
            finally:
                if not handed_over:
                    # a command that raised must not leave a tail task behind
                    self.robot.pop_deferred()
                    self.robot.task_lock.release()
        finally:
            self.robot_command_slots.release()




    def _run_deferred(self, tail_task: Callable[[], None]) -> None:
        """
        Runs the tail task in the background and releases the task lock once it is done.
        The lock is handed over to that thread instead of being reacquired by it, so the next
        SiLA command queues up behind the tail task instead of racing it for the robot.
        :param tail_task:
        :return:
        """
        def run_and_release() -> None:
            try:
                tail_task()
            except BaseException as error:
                logging.exception(f"{datetime.now()}: A deferred robot task failed")
                self.robot.report_deferred_failure(error)
            finally:
                self.robot.task_lock.release()

        Thread(target=run_and_release, daemon=True, name="deferred_robot_task").start()