423 lines
19 KiB
Python
423 lines
19 KiB
Python
"""Автоматы безопасного сеанса наземной станции и ровера для Lab041."""
|
||
|
||
from __future__ import annotations
|
||
|
||
from dataclasses import dataclass
|
||
from enum import Enum
|
||
import hashlib
|
||
|
||
from protocol.control_messages import (
|
||
ControlStateMessage,
|
||
EmergencyAck,
|
||
EmergencyStop,
|
||
MessageContext,
|
||
MessageType,
|
||
MovementAuthorizeAck,
|
||
MovementAuthorizeRequest,
|
||
ResetAck,
|
||
ResetRequest,
|
||
RoverSafetyCode,
|
||
SequenceComparison,
|
||
SessionHello,
|
||
SessionRejectReason,
|
||
SessionStatus,
|
||
compare_sequence,
|
||
)
|
||
|
||
|
||
class GroundSessionState(str, Enum):
|
||
GROUND_BOOT = "ground_boot"
|
||
GROUND_SYNCING = "ground_syncing"
|
||
GROUND_SAFE_SYNCHRONIZED = "ground_safe_synchronized"
|
||
GROUND_MOVEMENT_AUTH_REQUESTED = "ground_movement_auth_requested"
|
||
GROUND_ACTIVE = "ground_active"
|
||
GROUND_EMERGENCY_REQUESTED = "ground_emergency_requested"
|
||
GROUND_EMERGENCY_CONFIRMED = "ground_emergency_confirmed"
|
||
GROUND_RESET_REQUESTED = "ground_reset_requested"
|
||
|
||
|
||
class RoverSessionState(str, Enum):
|
||
ROVER_BOOT_SAFE = "rover_boot_safe"
|
||
ROVER_SESSION_SYNCED_SAFE = "rover_session_synced_safe"
|
||
ROVER_MOVEMENT_AUTHORIZED = "rover_movement_authorized"
|
||
STAGE1_DECELERATION = "stage1_deceleration"
|
||
STAGE2_BRAKING = "stage2_braking"
|
||
EMERGENCY_LATCHED = "emergency_latched"
|
||
|
||
|
||
class RejectionCause(str, Enum):
|
||
GROUND_SESSION_ID = "ground_session_id"
|
||
ROVER_BOOT_ID = "rover_boot_id"
|
||
CONTROL_EPOCH = "control_epoch"
|
||
SEQUENCE_NUMBER = "sequence_number"
|
||
EMERGENCY_EVENT_ID = "emergency_event_id"
|
||
RESET_REQUEST_ID = "reset_request_id"
|
||
NOT_SYNCHRONIZED = "not_synchronized"
|
||
MOVEMENT_NOT_AUTHORIZED = "movement_not_authorized"
|
||
|
||
|
||
@dataclass(frozen=True)
|
||
class ControlDecision:
|
||
accepted: bool
|
||
reason: RejectionCause | None
|
||
ambiguous_sequence: bool = False
|
||
|
||
|
||
@dataclass(frozen=True)
|
||
class ResetDecision:
|
||
acknowledgement: ResetAck
|
||
performed: bool
|
||
duplicate: bool
|
||
|
||
|
||
def deterministic_identifier(seed: int, purpose: str) -> int:
|
||
"""Получить воспроизводимый ненулевой 64-битный идентификатор."""
|
||
|
||
if not isinstance(seed, int):
|
||
raise TypeError("начальное значение генератора должно быть целым")
|
||
digest = hashlib.blake2b(f"Lab041:{purpose}:{seed}".encode("utf-8"), digest_size=8).digest()
|
||
return int.from_bytes(digest, "big") or 1
|
||
|
||
|
||
class GroundSessionController:
|
||
"""Наземный автомат без автоматического восстановления движения."""
|
||
|
||
def __init__(
|
||
self,
|
||
seed: int,
|
||
*,
|
||
persisted_emergency_intent: bool = False,
|
||
persisted_emergency_event_id: int = 0,
|
||
) -> None:
|
||
self.ground_session_id = deterministic_identifier(seed, "ground")
|
||
self.rover_boot_id = 0
|
||
self.control_epoch = 0
|
||
self.sequence_number = 0
|
||
self.state = GroundSessionState.GROUND_BOOT
|
||
self.emergency_intent = bool(persisted_emergency_intent)
|
||
self.emergency_event_id = persisted_emergency_event_id if self.emergency_intent else 0
|
||
self.authorization_id = 0
|
||
self.reset_request_id = 0
|
||
self.last_operator_sequence: int | None = None
|
||
self.movement_permitted = False
|
||
self.current_speed_mps = 0.0
|
||
self.zero_commands_after_restart = 0
|
||
|
||
def _next_sequence(self) -> int:
|
||
value = self.sequence_number
|
||
self.sequence_number = (self.sequence_number + 1) & 0xFFFFFFFF
|
||
return value
|
||
|
||
def _context(self, *, epoch: int | None = None) -> MessageContext:
|
||
return MessageContext(
|
||
self.ground_session_id,
|
||
self.rover_boot_id,
|
||
self.control_epoch if epoch is None else epoch,
|
||
self._next_sequence(),
|
||
)
|
||
|
||
def start_synchronization(self) -> SessionHello:
|
||
if self.state is not GroundSessionState.GROUND_BOOT:
|
||
raise RuntimeError("синхронизацию можно начать только после запуска")
|
||
self.state = GroundSessionState.GROUND_SYNCING
|
||
self.movement_permitted = False
|
||
self.current_speed_mps = 0.0
|
||
return SessionHello(self._context(epoch=0), self.emergency_intent, self.emergency_event_id)
|
||
|
||
def repeat_hello(self) -> SessionHello:
|
||
if self.state is not GroundSessionState.GROUND_SYNCING:
|
||
raise RuntimeError("повтор приветствия допустим только при синхронизации")
|
||
return SessionHello(self._context(epoch=0), self.emergency_intent, self.emergency_event_id)
|
||
|
||
def receive_status(self, status: SessionStatus) -> bool:
|
||
if self.state is not GroundSessionState.GROUND_SYNCING:
|
||
return False
|
||
if status.context.ground_session_id != self.ground_session_id:
|
||
return False
|
||
if status.context.rover_boot_id == 0:
|
||
return False
|
||
self.rover_boot_id = status.context.rover_boot_id
|
||
self.control_epoch = 0
|
||
self.movement_permitted = False
|
||
self.current_speed_mps = 0.0
|
||
if self.emergency_intent:
|
||
self.state = GroundSessionState.GROUND_EMERGENCY_REQUESTED
|
||
else:
|
||
self.state = GroundSessionState.GROUND_SAFE_SYNCHRONIZED
|
||
return True
|
||
|
||
def request_movement_authorization(self, authorization_id: int) -> MovementAuthorizeRequest:
|
||
if self.state is not GroundSessionState.GROUND_SAFE_SYNCHRONIZED:
|
||
raise RuntimeError("разрешение движения требует безопасного согласованного сеанса")
|
||
if self.emergency_intent:
|
||
raise RuntimeError("при аварийном намерении движение запрещено")
|
||
if not 0 <= authorization_id <= 0xFFFFFFFFFFFFFFFF:
|
||
raise ValueError("authorization_id вне диапазона uint64")
|
||
self.authorization_id = authorization_id
|
||
self.state = GroundSessionState.GROUND_MOVEMENT_AUTH_REQUESTED
|
||
return MovementAuthorizeRequest(self._context(epoch=0), authorization_id)
|
||
|
||
def receive_movement_authorize_ack(self, acknowledgement: MovementAuthorizeAck) -> bool:
|
||
if self.state is not GroundSessionState.GROUND_MOVEMENT_AUTH_REQUESTED:
|
||
return False
|
||
context = acknowledgement.context
|
||
if (
|
||
context.ground_session_id != self.ground_session_id
|
||
or context.rover_boot_id != self.rover_boot_id
|
||
or acknowledgement.authorization_id != self.authorization_id
|
||
or not acknowledgement.accepted
|
||
or context.control_epoch == 0
|
||
):
|
||
return False
|
||
self.control_epoch = context.control_epoch
|
||
self.state = GroundSessionState.GROUND_ACTIVE
|
||
self.movement_permitted = False
|
||
self.current_speed_mps = 0.0
|
||
self.last_operator_sequence = None
|
||
return True
|
||
|
||
def new_operator_control(self, speed_mps: float, turn: float = 0.0) -> ControlStateMessage:
|
||
if self.state is not GroundSessionState.GROUND_ACTIVE:
|
||
raise RuntimeError("новая команда движения возможна только в активном сеансе")
|
||
if speed_mps <= 0.0:
|
||
raise ValueError("скорость новой команды должна быть положительной")
|
||
context = self._context()
|
||
self.last_operator_sequence = context.sequence_number
|
||
self.movement_permitted = True
|
||
self.current_speed_mps = speed_mps
|
||
return ControlStateMessage(context, speed_mps, turn, False, True)
|
||
|
||
def safe_zero_control(self) -> ControlStateMessage:
|
||
self.zero_commands_after_restart += 1
|
||
return ControlStateMessage(self._context(), 0.0, 0.0, True, False)
|
||
|
||
def request_emergency(self, emergency_event_id: int) -> EmergencyStop:
|
||
if not 0 < emergency_event_id <= 0xFFFFFFFFFFFFFFFF:
|
||
raise ValueError("emergency_event_id должен быть ненулевым uint64")
|
||
self.emergency_intent = True
|
||
self.emergency_event_id = emergency_event_id
|
||
self.movement_permitted = False
|
||
self.current_speed_mps = 0.0
|
||
self.state = GroundSessionState.GROUND_EMERGENCY_REQUESTED
|
||
return EmergencyStop(self._context(), emergency_event_id)
|
||
|
||
def receive_emergency_ack(self, acknowledgement: EmergencyAck) -> bool:
|
||
if acknowledgement.context.ground_session_id != self.ground_session_id:
|
||
return False
|
||
if acknowledgement.emergency_event_id != self.emergency_event_id:
|
||
return False
|
||
if self.state is GroundSessionState.GROUND_EMERGENCY_REQUESTED:
|
||
self.state = GroundSessionState.GROUND_EMERGENCY_CONFIRMED
|
||
return True
|
||
|
||
def request_reset(self, reset_request_id: int) -> ResetRequest:
|
||
if not self.emergency_intent:
|
||
raise RuntimeError("сброс требует аварийного намерения")
|
||
if not 0 < reset_request_id <= 0xFFFFFFFFFFFFFFFF:
|
||
raise ValueError("reset_request_id должен быть ненулевым uint64")
|
||
self.reset_request_id = reset_request_id
|
||
self.state = GroundSessionState.GROUND_RESET_REQUESTED
|
||
return ResetRequest(self._context(), self.emergency_event_id, reset_request_id, 0.0, False)
|
||
|
||
def receive_reset_ack(self, acknowledgement: ResetAck) -> bool:
|
||
if self.state is not GroundSessionState.GROUND_RESET_REQUESTED:
|
||
return False
|
||
if (
|
||
acknowledgement.context.ground_session_id != self.ground_session_id
|
||
or acknowledgement.context.rover_boot_id != self.rover_boot_id
|
||
or acknowledgement.emergency_event_id != self.emergency_event_id
|
||
or acknowledgement.reset_request_id != self.reset_request_id
|
||
or not acknowledgement.accepted
|
||
):
|
||
return False
|
||
self.emergency_intent = False
|
||
self.emergency_event_id = 0
|
||
self.movement_permitted = False
|
||
self.current_speed_mps = 0.0
|
||
self.control_epoch = 0
|
||
self.state = GroundSessionState.GROUND_SAFE_SYNCHRONIZED
|
||
return True
|
||
|
||
|
||
class RoverSessionController:
|
||
"""Ровер, который после каждого запуска начинает с безопасного запрета."""
|
||
|
||
def __init__(self, seed: int) -> None:
|
||
self.rover_boot_id = deterministic_identifier(seed, "rover")
|
||
self.ground_session_id = 0
|
||
self.control_epoch = 0
|
||
self.state = RoverSessionState.ROVER_BOOT_SAFE
|
||
self.speed_mps = 0.0
|
||
self.movement_permitted = False
|
||
self.emergency_event_id = 0
|
||
self.last_sequences: dict[tuple[int, int, int, int], int] = {}
|
||
self.accepted_reset_keys: set[tuple[int, int, int, int]] = set()
|
||
self.duplicate_hellos = 0
|
||
self.duplicate_resets = 0
|
||
|
||
@property
|
||
def safe(self) -> bool:
|
||
return self.speed_mps == 0.0 and not self.movement_permitted
|
||
|
||
def _status_context(self, sequence_number: int = 0) -> MessageContext:
|
||
return MessageContext(self.ground_session_id, self.rover_boot_id, self.control_epoch, sequence_number)
|
||
|
||
def receive_hello(self, hello: SessionHello) -> SessionStatus:
|
||
if hello.context.ground_session_id == 0:
|
||
raise ValueError("ground_session_id должен быть ненулевым")
|
||
if hello.context.ground_session_id == self.ground_session_id:
|
||
self.duplicate_hellos += 1
|
||
else:
|
||
self.ground_session_id = hello.context.ground_session_id
|
||
self.control_epoch = 0
|
||
self.last_sequences.clear()
|
||
self.movement_permitted = False
|
||
self.speed_mps = 0.0
|
||
self.state = RoverSessionState.ROVER_SESSION_SYNCED_SAFE
|
||
if hello.emergency_intent:
|
||
self.emergency_event_id = hello.emergency_event_id
|
||
self.state = RoverSessionState.EMERGENCY_LATCHED
|
||
code = {
|
||
RoverSessionState.ROVER_BOOT_SAFE: RoverSafetyCode.ROVER_BOOT_SAFE,
|
||
RoverSessionState.ROVER_SESSION_SYNCED_SAFE: RoverSafetyCode.ROVER_SESSION_SYNCED_SAFE,
|
||
RoverSessionState.ROVER_MOVEMENT_AUTHORIZED: RoverSafetyCode.ROVER_MOVEMENT_AUTHORIZED,
|
||
RoverSessionState.STAGE1_DECELERATION: RoverSafetyCode.STAGE1_DECELERATION,
|
||
RoverSessionState.STAGE2_BRAKING: RoverSafetyCode.STAGE2_BRAKING,
|
||
RoverSessionState.EMERGENCY_LATCHED: RoverSafetyCode.EMERGENCY_LATCHED,
|
||
}[self.state]
|
||
return SessionStatus(
|
||
self._status_context(hello.context.sequence_number),
|
||
code,
|
||
self.state is RoverSessionState.EMERGENCY_LATCHED,
|
||
self.emergency_event_id,
|
||
)
|
||
|
||
def authorize_movement(self, request: MovementAuthorizeRequest) -> MovementAuthorizeAck:
|
||
accepted = (
|
||
self.state is RoverSessionState.ROVER_SESSION_SYNCED_SAFE
|
||
and request.context.ground_session_id == self.ground_session_id
|
||
and request.context.rover_boot_id == self.rover_boot_id
|
||
and self.emergency_event_id == 0
|
||
)
|
||
if accepted:
|
||
self.control_epoch = (self.control_epoch + 1) & 0xFFFFFFFF
|
||
if self.control_epoch == 0:
|
||
self.control_epoch = 1
|
||
self.state = RoverSessionState.ROVER_MOVEMENT_AUTHORIZED
|
||
self.speed_mps = 0.0
|
||
self.movement_permitted = False
|
||
return MovementAuthorizeAck(
|
||
MessageContext(self.ground_session_id, self.rover_boot_id, self.control_epoch, request.context.sequence_number),
|
||
request.authorization_id,
|
||
accepted,
|
||
)
|
||
|
||
def receive_control(self, message: ControlStateMessage, stream_id: int) -> ControlDecision:
|
||
context = message.context
|
||
if context.ground_session_id != self.ground_session_id:
|
||
return ControlDecision(False, RejectionCause.GROUND_SESSION_ID)
|
||
if context.rover_boot_id != self.rover_boot_id:
|
||
return ControlDecision(False, RejectionCause.ROVER_BOOT_ID)
|
||
if context.control_epoch != self.control_epoch:
|
||
return ControlDecision(False, RejectionCause.CONTROL_EPOCH)
|
||
if self.state is RoverSessionState.EMERGENCY_LATCHED:
|
||
return ControlDecision(False, RejectionCause.MOVEMENT_NOT_AUTHORIZED)
|
||
if self.state is not RoverSessionState.ROVER_MOVEMENT_AUTHORIZED:
|
||
return ControlDecision(False, RejectionCause.MOVEMENT_NOT_AUTHORIZED)
|
||
key = (context.ground_session_id, context.rover_boot_id, context.control_epoch, stream_id)
|
||
old = self.last_sequences.get(key)
|
||
if old is not None:
|
||
comparison = compare_sequence(context.sequence_number, old)
|
||
if comparison is SequenceComparison.AMBIGUOUS:
|
||
return ControlDecision(False, RejectionCause.SEQUENCE_NUMBER, True)
|
||
if comparison is not SequenceComparison.NEWER:
|
||
return ControlDecision(False, RejectionCause.SEQUENCE_NUMBER)
|
||
self.last_sequences[key] = context.sequence_number
|
||
self.speed_mps = message.desired_speed_mps
|
||
self.movement_permitted = message.movement_permitted
|
||
return ControlDecision(True, None)
|
||
|
||
def receive_emergency(self, message: EmergencyStop) -> EmergencyAck | None:
|
||
if (
|
||
message.context.ground_session_id != self.ground_session_id
|
||
or message.context.rover_boot_id != self.rover_boot_id
|
||
):
|
||
return None
|
||
self.emergency_event_id = message.emergency_event_id
|
||
self.state = RoverSessionState.EMERGENCY_LATCHED
|
||
self.speed_mps = 0.0
|
||
self.movement_permitted = False
|
||
return EmergencyAck(
|
||
MessageContext(self.ground_session_id, self.rover_boot_id, self.control_epoch, message.context.sequence_number),
|
||
message.emergency_event_id,
|
||
)
|
||
|
||
def receive_reset(self, request: ResetRequest) -> ResetDecision:
|
||
context = request.context
|
||
reason = SessionRejectReason.NOT_SYNCHRONIZED
|
||
if context.ground_session_id != self.ground_session_id:
|
||
reason = SessionRejectReason.GROUND_SESSION_ID
|
||
elif context.rover_boot_id != self.rover_boot_id:
|
||
reason = SessionRejectReason.ROVER_BOOT_ID
|
||
elif request.emergency_event_id != self.emergency_event_id:
|
||
reason = SessionRejectReason.EMERGENCY_EVENT_ID
|
||
elif self.speed_mps != 0.0:
|
||
reason = SessionRejectReason.MOVEMENT_NOT_AUTHORIZED
|
||
else:
|
||
reason = SessionRejectReason.RESET_REQUEST_ID
|
||
key = (
|
||
context.ground_session_id,
|
||
context.rover_boot_id,
|
||
request.emergency_event_id,
|
||
request.reset_request_id,
|
||
)
|
||
if key in self.accepted_reset_keys:
|
||
self.duplicate_resets += 1
|
||
acknowledgement = ResetAck(context, request.emergency_event_id, request.reset_request_id, True, SessionRejectReason.ACCEPTED)
|
||
return ResetDecision(acknowledgement, False, True)
|
||
accepted = (
|
||
context.ground_session_id == self.ground_session_id
|
||
and context.rover_boot_id == self.rover_boot_id
|
||
and self.state is RoverSessionState.EMERGENCY_LATCHED
|
||
and request.emergency_event_id == self.emergency_event_id
|
||
and request.requested_speed_mps == 0.0
|
||
and not request.movement_permitted
|
||
and self.speed_mps == 0.0
|
||
)
|
||
if accepted:
|
||
self.accepted_reset_keys.add(key)
|
||
self.emergency_event_id = 0
|
||
self.state = RoverSessionState.ROVER_SESSION_SYNCED_SAFE
|
||
self.speed_mps = 0.0
|
||
self.movement_permitted = False
|
||
reason = SessionRejectReason.ACCEPTED
|
||
acknowledgement = ResetAck(context, request.emergency_event_id, request.reset_request_id, accepted, reason)
|
||
return ResetDecision(acknowledgement, accepted, False)
|
||
|
||
def enter_watchdog_stage1(self) -> None:
|
||
if self.state is RoverSessionState.ROVER_MOVEMENT_AUTHORIZED:
|
||
self.state = RoverSessionState.STAGE1_DECELERATION
|
||
self.movement_permitted = False
|
||
|
||
def enter_watchdog_stage2(self) -> None:
|
||
if self.state in (RoverSessionState.ROVER_MOVEMENT_AUTHORIZED, RoverSessionState.STAGE1_DECELERATION):
|
||
self.state = RoverSessionState.STAGE2_BRAKING
|
||
self.movement_permitted = False
|
||
|
||
|
||
def rejection_reason_for_message(reason: RejectionCause) -> SessionRejectReason:
|
||
return {
|
||
RejectionCause.GROUND_SESSION_ID: SessionRejectReason.GROUND_SESSION_ID,
|
||
RejectionCause.ROVER_BOOT_ID: SessionRejectReason.ROVER_BOOT_ID,
|
||
RejectionCause.CONTROL_EPOCH: SessionRejectReason.CONTROL_EPOCH,
|
||
RejectionCause.SEQUENCE_NUMBER: SessionRejectReason.SEQUENCE_NUMBER,
|
||
RejectionCause.EMERGENCY_EVENT_ID: SessionRejectReason.EMERGENCY_EVENT_ID,
|
||
RejectionCause.RESET_REQUEST_ID: SessionRejectReason.RESET_REQUEST_ID,
|
||
RejectionCause.NOT_SYNCHRONIZED: SessionRejectReason.NOT_SYNCHRONIZED,
|
||
RejectionCause.MOVEMENT_NOT_AUTHORIZED: SessionRejectReason.MOVEMENT_NOT_AUTHORIZED,
|
||
}[reason]
|
||
|
||
|
||
assert len({state.value for state in GroundSessionState}) == len(GroundSessionState)
|
||
assert len({state.value for state in RoverSessionState}) == len(RoverSessionState)
|