fischertechnik-factory-twin/simulator/devs_models/mpo_gripper.py

from data_models.mqtt_message import MqttMessage
from pypdevs.DEVS import AtomicDEVS
from pypdevs.infinity import INFINITY
from dataclasses import dataclass
from loguru import logger

from data_models.workpiece import Workpiece
from utils.get_timestamp import get_timestamp
from utils.timed_phase_enum import TimedPhaseEnum


class MPOGrPhase(TimedPhaseEnum):
    """ Steps in order, along with their timings """
    IDLE = ('IDLE', INFINITY)
    MOVE_TO_OVEN = ('MOVE_TO_OVEN', 10.938)
    AWAIT_TABLE_TURN = ('AWAIT_TABLE_TURN', INFINITY) # wait for saw table to turn
    PICKUP_WP = ('PICKUP_WP', 2.201) # Pickup wp from the oven (move down and grip)
    AWAIT_WP = ('AWAIT_WP', INFINITY) # Await oven to give us the workpiece (should be instant)
    MOVE_TO_SAW_TABLE = ('MOVE_TO_SAW_TABLE', 12.122) # move to saw table and output workpiece


@dataclass
class MPOGrState:
    delta_t: float = INFINITY
    phase: MPOGrPhase = MPOGrPhase.IDLE
    workpiece: Workpiece | None = None
    status_requested: bool = True  # if true, publish status
    visual_update_pending: bool = False


class MPOGripper(AtomicDEVS):
    """ MPO vacuum gripper to transport workpieces from the oven to the table saw """

    def __init__(self, name: str):
        super(MPOGripper, self).__init__(name)
        self.oven_in = self.addInPort("oven_in") # input for oven giving workpiece
        self.oven_out = self.addOutPort("oven_out") # output for requesting wp from oven

        self.table_in = self.addInPort("table_in") # input for getting table saw turning confirmation
        self.table_out = self.addOutPort("table_out") # output to push wp to the table saw

        self.mqtt_in = self.addInPort("mqtt_in")  # input for MQTT messages (e.g. visual refresh request)
        self.mqtt_out = self.addOutPort("mqtt_out")

        self.state = MPOGrState()

    def change_phase(self, new_phase: MPOGrPhase):
        """ Wrapper for changing the phase and time associated with it, helps with logging """
        self.state.phase = new_phase
        self.state.delta_t = new_phase.timing
        logger.trace(f"{type(self).__name__} '{self.name}' phase changed to {new_phase}")
        self.state.visual_update_pending = True

    def get_visual_update_data(self) -> MqttMessage:
        """ Get visual update data for the animation, contains the action taken and the duration of that action left """
        message = MqttMessage()
        message.topic = "visualization/mpo/gripper"
        duration = self.state.delta_t
        if duration == INFINITY: duration = None
        message.payload = {
            "action": self.state.phase.value,
            "duration": duration,  # should be equal to the timing of the phase
            "workpiece": self.state.workpiece.to_dict() if self.state.workpiece else None,
        }
        return message

    def get_status_message(self, use_next_phase=True) -> MqttMessage:
        phase = self.state.phase.next() if use_next_phase else self.state.phase
        active = 0 if (phase == MPOGrPhase.IDLE) else 1
        code = 2 if active == 1 else 1

        message = MqttMessage()
        message.topic = "f/i/state/mpo"
        message.payload = {
            "active": active,
            "code": code,
            "description": '',
            "station": 'mpo',
            "ts": get_timestamp(),
        }
        return message

    def extTransition(self, inputs):
        self.state.delta_t -= self.elapsed

        if self.mqtt_in in inputs:
            msg = inputs[self.mqtt_in][0]
            if msg.topic == "simulation/ctrl/all" and msg.payload.get("action") == "refresh":
                self.state.status_requested = True
                self.state.visual_update_pending = True
                return self.state

        elif not (self.state.phase == MPOGrPhase.IDLE or self.state.phase == MPOGrPhase.AWAIT_TABLE_TURN or self.state.phase == MPOGrPhase.AWAIT_WP):
            logger.error(f"{type(self).__name__} '{self.name}' received input while not expecting: {inputs}")
            return self.state

        if self.oven_in in inputs:
            # 2 options; workpiece or instruction to pickup
            if self.state.phase == MPOGrPhase.IDLE:
                self.change_phase(MPOGrPhase.MOVE_TO_OVEN)  # start moving to oven as requested
            elif self.state.phase == MPOGrPhase.AWAIT_WP:
                wp = inputs[self.oven_in][0]
                self.state.workpiece = wp  # pickup new workpiece
                self.change_phase(MPOGrPhase.MOVE_TO_SAW_TABLE)  # move workpiece to saw table
            else:
                logger.error(f"{type(self).__name__} '{self.name}' received oven input while not expecting: {inputs}")

        elif self.table_in in inputs:
            # Only option is if we are awaiting the saw table to turn to its rest position
            if self.state.phase == MPOGrPhase.AWAIT_TABLE_TURN:
                self.change_phase(MPOGrPhase.PICKUP_WP)
            else:
                logger.error(f"{type(self).__name__} '{self.name}' received table saw input while not expecting: {inputs}")

        return self.state # important, return state

    def timeAdvance(self):
        if self.state.visual_update_pending or self.state.status_requested:
            return 0.0
        return self.state.delta_t

    def outputFnc(self):
        if self.state.visual_update_pending:
            return {self.mqtt_out: [self.get_visual_update_data()]}
        if self.state.status_requested:
            if self.state.phase == MPOGrPhase.IDLE:
                return {} # do not output status when idle (other MPO parts likely not idle)
            return {self.mqtt_out: [self.get_status_message(use_next_phase=False)]}

        if self.state.phase == MPOGrPhase.MOVE_TO_OVEN:
            return {self.table_out: {}, self.mqtt_out: [self.get_status_message()]} # signal Table Saw to turn its table to face us
        elif self.state.phase == MPOGrPhase.PICKUP_WP:
            return {self.oven_out: {}, self.mqtt_out: [self.get_status_message()]} # signal oven that we are ready to pick up the workpiece
        elif self.state.phase == MPOGrPhase.MOVE_TO_SAW_TABLE:
            return {self.table_out: [self.state.workpiece]} # output workpiece to table, do not signal own status

        return {self.mqtt_out: [self.get_status_message()]}

    def intTransition(self):
        if self.state.visual_update_pending:
            self.state.visual_update_pending = False
            return self.state
        if self.state.status_requested:
            self.state.status_requested = False
            return self.state

        if self.state.phase == MPOGrPhase.MOVE_TO_SAW_TABLE:
            self.state.workpiece = None # remove the workpiece we just output to the table saw

        # Transition to next state
        self.change_phase(self.state.phase.next())
        return self.state