Lab041: add session and restart safety
This commit is contained in:
LittleSam129
422
protocol/session_state.py
Normal file
422
protocol/session_state.py
Normal file
@@ -0,0 +1,422 @@
|
||||
"""Автоматы безопасного сеанса наземной станции и ровера для 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)
|
||||
Reference in New Issue
Block a user