Lab041: add session and restart safety
This commit is contained in:
LittleSam129
434
protocol/control_messages.py
Normal file
434
protocol/control_messages.py
Normal file
@@ -0,0 +1,434 @@
|
||||
"""Прикладные сообщения безопасного сеанса Lab041.
|
||||
|
||||
Общий заголовок полезной нагрузки имеет формат ``!BBQQII``: версия, тип
|
||||
сообщения, 64-битные идентификаторы запуска наземной станции и ровера,
|
||||
32-битный период управления и 32-битный номер сообщения. Порядок байтов
|
||||
сетевой, неявного выравнивания нет.
|
||||
|
||||
Отдельная контрольная сумма намеренно не добавляется: контрольная сумма
|
||||
общего пакета Lab033 уже защищает заголовок и всю прикладную нагрузку. После
|
||||
проверки этой суммы всё равно требуется приведённая здесь смысловая проверка.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import IntEnum
|
||||
import math
|
||||
import struct
|
||||
from typing import Union
|
||||
|
||||
|
||||
MESSAGE_VERSION = 1
|
||||
COMMON_FORMAT = "!BBQQII"
|
||||
COMMON_SIZE = struct.calcsize(COMMON_FORMAT)
|
||||
UINT32_MODULUS = 1 << 32
|
||||
UINT32_HALF_RANGE = 1 << 31
|
||||
MAX_CONTROL_SPEED_MPS = 15.0
|
||||
MAX_ABS_TURN = 1.0
|
||||
|
||||
|
||||
class ControlMessageError(ValueError):
|
||||
"""Ошибка формата или смысловой проверки прикладного сообщения."""
|
||||
|
||||
|
||||
class MessageType(IntEnum):
|
||||
SESSION_HELLO = 1
|
||||
SESSION_STATUS = 2
|
||||
MOVEMENT_AUTHORIZE_REQUEST = 3
|
||||
MOVEMENT_AUTHORIZE_ACK = 4
|
||||
CONTROL_STATE = 5
|
||||
EMERGENCY_STOP = 6
|
||||
EMERGENCY_ACK = 7
|
||||
RESET_REQUEST = 8
|
||||
RESET_ACK = 9
|
||||
SESSION_REJECT = 10
|
||||
|
||||
|
||||
class RoverSafetyCode(IntEnum):
|
||||
ROVER_BOOT_SAFE = 1
|
||||
ROVER_SESSION_SYNCED_SAFE = 2
|
||||
ROVER_MOVEMENT_AUTHORIZED = 3
|
||||
STAGE1_DECELERATION = 4
|
||||
STAGE2_BRAKING = 5
|
||||
EMERGENCY_LATCHED = 6
|
||||
|
||||
|
||||
class SessionRejectReason(IntEnum):
|
||||
ACCEPTED = 0
|
||||
GROUND_SESSION_ID = 1
|
||||
ROVER_BOOT_ID = 2
|
||||
CONTROL_EPOCH = 3
|
||||
SEQUENCE_NUMBER = 4
|
||||
EMERGENCY_EVENT_ID = 5
|
||||
RESET_REQUEST_ID = 6
|
||||
NOT_SYNCHRONIZED = 7
|
||||
MOVEMENT_NOT_AUTHORIZED = 8
|
||||
|
||||
|
||||
class SequenceComparison(IntEnum):
|
||||
NOT_NEWER = 0
|
||||
NEWER = 1
|
||||
AMBIGUOUS = 2
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MessageContext:
|
||||
ground_session_id: int
|
||||
rover_boot_id: int
|
||||
control_epoch: int
|
||||
sequence_number: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SessionHello:
|
||||
context: MessageContext
|
||||
emergency_intent: bool
|
||||
emergency_event_id: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SessionStatus:
|
||||
context: MessageContext
|
||||
safety_state: RoverSafetyCode
|
||||
emergency_latched: bool
|
||||
emergency_event_id: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MovementAuthorizeRequest:
|
||||
context: MessageContext
|
||||
authorization_id: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MovementAuthorizeAck:
|
||||
context: MessageContext
|
||||
authorization_id: int
|
||||
accepted: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ControlStateMessage:
|
||||
context: MessageContext
|
||||
desired_speed_mps: float
|
||||
desired_turn: float
|
||||
braking: bool
|
||||
movement_permitted: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EmergencyStop:
|
||||
context: MessageContext
|
||||
emergency_event_id: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EmergencyAck:
|
||||
context: MessageContext
|
||||
emergency_event_id: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ResetRequest:
|
||||
context: MessageContext
|
||||
emergency_event_id: int
|
||||
reset_request_id: int
|
||||
requested_speed_mps: float
|
||||
movement_permitted: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ResetAck:
|
||||
context: MessageContext
|
||||
emergency_event_id: int
|
||||
reset_request_id: int
|
||||
accepted: bool
|
||||
reason: SessionRejectReason
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SessionReject:
|
||||
context: MessageContext
|
||||
rejected_type: MessageType
|
||||
reason: SessionRejectReason
|
||||
|
||||
|
||||
ControlMessage = Union[
|
||||
SessionHello,
|
||||
SessionStatus,
|
||||
MovementAuthorizeRequest,
|
||||
MovementAuthorizeAck,
|
||||
ControlStateMessage,
|
||||
EmergencyStop,
|
||||
EmergencyAck,
|
||||
ResetRequest,
|
||||
ResetAck,
|
||||
SessionReject,
|
||||
]
|
||||
|
||||
|
||||
_TYPE_BY_CLASS = {
|
||||
SessionHello: MessageType.SESSION_HELLO,
|
||||
SessionStatus: MessageType.SESSION_STATUS,
|
||||
MovementAuthorizeRequest: MessageType.MOVEMENT_AUTHORIZE_REQUEST,
|
||||
MovementAuthorizeAck: MessageType.MOVEMENT_AUTHORIZE_ACK,
|
||||
ControlStateMessage: MessageType.CONTROL_STATE,
|
||||
EmergencyStop: MessageType.EMERGENCY_STOP,
|
||||
EmergencyAck: MessageType.EMERGENCY_ACK,
|
||||
ResetRequest: MessageType.RESET_REQUEST,
|
||||
ResetAck: MessageType.RESET_ACK,
|
||||
SessionReject: MessageType.SESSION_REJECT,
|
||||
}
|
||||
|
||||
|
||||
def _unsigned(value: int, maximum: int, name: str) -> int:
|
||||
if not isinstance(value, int) or isinstance(value, bool) or not 0 <= value <= maximum:
|
||||
raise ControlMessageError(f"{name} выходит за допустимый беззнаковый диапазон")
|
||||
return value
|
||||
|
||||
|
||||
def validate_context(context: MessageContext) -> MessageContext:
|
||||
if not isinstance(context, MessageContext):
|
||||
raise TypeError("требуется MessageContext")
|
||||
_unsigned(context.ground_session_id, 0xFFFFFFFFFFFFFFFF, "ground_session_id")
|
||||
_unsigned(context.rover_boot_id, 0xFFFFFFFFFFFFFFFF, "rover_boot_id")
|
||||
_unsigned(context.control_epoch, 0xFFFFFFFF, "control_epoch")
|
||||
_unsigned(context.sequence_number, 0xFFFFFFFF, "sequence_number")
|
||||
return context
|
||||
|
||||
|
||||
def compare_sequence(new: int, old: int) -> SequenceComparison:
|
||||
"""Сравнить 32-битные номера по модулю с неоднозначной серединой."""
|
||||
|
||||
_unsigned(new, 0xFFFFFFFF, "new sequence_number")
|
||||
_unsigned(old, 0xFFFFFFFF, "old sequence_number")
|
||||
delta = (new - old) % UINT32_MODULUS
|
||||
if delta == UINT32_HALF_RANGE:
|
||||
return SequenceComparison.AMBIGUOUS
|
||||
if 0 < delta < UINT32_HALF_RANGE:
|
||||
return SequenceComparison.NEWER
|
||||
return SequenceComparison.NOT_NEWER
|
||||
|
||||
|
||||
def _common(message_type: MessageType, context: MessageContext) -> bytes:
|
||||
validate_context(context)
|
||||
return struct.pack(
|
||||
COMMON_FORMAT,
|
||||
MESSAGE_VERSION,
|
||||
int(message_type),
|
||||
context.ground_session_id,
|
||||
context.rover_boot_id,
|
||||
context.control_epoch,
|
||||
context.sequence_number,
|
||||
)
|
||||
|
||||
|
||||
def _boolean(value: bool, name: str) -> int:
|
||||
if not isinstance(value, bool):
|
||||
raise ControlMessageError(f"{name} должен быть логическим значением")
|
||||
return int(value)
|
||||
|
||||
|
||||
def _finite(value: float, name: str) -> float:
|
||||
value = float(value)
|
||||
if not math.isfinite(value):
|
||||
raise ControlMessageError(f"{name} должен быть конечным числом")
|
||||
return value
|
||||
|
||||
|
||||
def encode_message(message: ControlMessage) -> bytes:
|
||||
"""Проверить и сериализовать одно прикладное сообщение."""
|
||||
|
||||
message_type = _TYPE_BY_CLASS.get(type(message))
|
||||
if message_type is None:
|
||||
raise TypeError("неподдерживаемый класс прикладного сообщения")
|
||||
prefix = _common(message_type, message.context)
|
||||
if isinstance(message, SessionHello):
|
||||
event = _unsigned(message.emergency_event_id, 0xFFFFFFFFFFFFFFFF, "emergency_event_id")
|
||||
return prefix + struct.pack("!BQ", _boolean(message.emergency_intent, "emergency_intent"), event)
|
||||
if isinstance(message, SessionStatus):
|
||||
try:
|
||||
state = RoverSafetyCode(message.safety_state)
|
||||
except ValueError as error:
|
||||
raise ControlMessageError("неизвестное состояние безопасности ровера") from error
|
||||
event = _unsigned(message.emergency_event_id, 0xFFFFFFFFFFFFFFFF, "emergency_event_id")
|
||||
return prefix + struct.pack("!BBQ", int(state), _boolean(message.emergency_latched, "emergency_latched"), event)
|
||||
if isinstance(message, MovementAuthorizeRequest):
|
||||
return prefix + struct.pack("!Q", _unsigned(message.authorization_id, 0xFFFFFFFFFFFFFFFF, "authorization_id"))
|
||||
if isinstance(message, MovementAuthorizeAck):
|
||||
return prefix + struct.pack(
|
||||
"!QB",
|
||||
_unsigned(message.authorization_id, 0xFFFFFFFFFFFFFFFF, "authorization_id"),
|
||||
_boolean(message.accepted, "accepted"),
|
||||
)
|
||||
if isinstance(message, ControlStateMessage):
|
||||
speed = _finite(message.desired_speed_mps, "desired_speed_mps")
|
||||
turn = _finite(message.desired_turn, "desired_turn")
|
||||
if not 0.0 <= speed <= MAX_CONTROL_SPEED_MPS:
|
||||
raise ControlMessageError("требуемая скорость вне диапазона 0…15 м/с")
|
||||
if not -MAX_ABS_TURN <= turn <= MAX_ABS_TURN:
|
||||
raise ControlMessageError("требуемый поворот вне диапазона -1…1")
|
||||
braking = _boolean(message.braking, "braking")
|
||||
permitted = _boolean(message.movement_permitted, "movement_permitted")
|
||||
if (braking or not permitted) and speed != 0.0:
|
||||
raise ControlMessageError("при торможении или запрете движения скорость должна быть нулевой")
|
||||
if braking and permitted:
|
||||
raise ControlMessageError("торможение несовместимо с разрешением движения")
|
||||
return prefix + struct.pack("!ffBB", speed, turn, braking, permitted)
|
||||
if isinstance(message, (EmergencyStop, EmergencyAck)):
|
||||
event = _unsigned(message.emergency_event_id, 0xFFFFFFFFFFFFFFFF, "emergency_event_id")
|
||||
return prefix + struct.pack("!Q", event)
|
||||
if isinstance(message, ResetRequest):
|
||||
event = _unsigned(message.emergency_event_id, 0xFFFFFFFFFFFFFFFF, "emergency_event_id")
|
||||
request = _unsigned(message.reset_request_id, 0xFFFFFFFFFFFFFFFF, "reset_request_id")
|
||||
speed = _finite(message.requested_speed_mps, "requested_speed_mps")
|
||||
permitted = _boolean(message.movement_permitted, "movement_permitted")
|
||||
if speed != 0.0 or permitted:
|
||||
raise ControlMessageError("безопасный сброс требует нулевой скорости и запрета движения")
|
||||
return prefix + struct.pack("!QQfB", event, request, speed, permitted)
|
||||
if isinstance(message, ResetAck):
|
||||
try:
|
||||
reason = SessionRejectReason(message.reason)
|
||||
except ValueError as error:
|
||||
raise ControlMessageError("неизвестная причина подтверждения сброса") from error
|
||||
return prefix + struct.pack(
|
||||
"!QQBB",
|
||||
_unsigned(message.emergency_event_id, 0xFFFFFFFFFFFFFFFF, "emergency_event_id"),
|
||||
_unsigned(message.reset_request_id, 0xFFFFFFFFFFFFFFFF, "reset_request_id"),
|
||||
_boolean(message.accepted, "accepted"),
|
||||
int(reason),
|
||||
)
|
||||
if isinstance(message, SessionReject):
|
||||
try:
|
||||
rejected_type = MessageType(message.rejected_type)
|
||||
reason = SessionRejectReason(message.reason)
|
||||
except ValueError as error:
|
||||
raise ControlMessageError("неизвестный код отказа сеанса") from error
|
||||
return prefix + struct.pack("!BB", int(rejected_type), int(reason))
|
||||
raise AssertionError("необработанный тип сообщения")
|
||||
|
||||
|
||||
def _decode_bool(value: int, name: str) -> bool:
|
||||
if value not in (0, 1):
|
||||
raise ControlMessageError(f"{name} должен быть равен нулю или единице")
|
||||
return bool(value)
|
||||
|
||||
|
||||
def _unpack_exact(body: bytes, format_string: str) -> tuple[object, ...]:
|
||||
expected = struct.calcsize(format_string)
|
||||
if len(body) != expected:
|
||||
raise ControlMessageError(f"неверная длина тела сообщения: {len(body)}, ожидается {expected}")
|
||||
return struct.unpack(format_string, body)
|
||||
|
||||
|
||||
def decode_message(payload: bytes) -> ControlMessage:
|
||||
"""Разобрать сообщение и выполнить полную смысловую проверку."""
|
||||
|
||||
if not isinstance(payload, (bytes, bytearray)):
|
||||
raise TypeError("полезная нагрузка должна быть последовательностью байтов")
|
||||
payload = bytes(payload)
|
||||
if len(payload) < COMMON_SIZE:
|
||||
raise ControlMessageError("сообщение короче общего заголовка")
|
||||
version, raw_type, ground, rover, epoch, sequence = struct.unpack(COMMON_FORMAT, payload[:COMMON_SIZE])
|
||||
if version != MESSAGE_VERSION:
|
||||
raise ControlMessageError("неподдерживаемая версия прикладного сообщения")
|
||||
try:
|
||||
message_type = MessageType(raw_type)
|
||||
except ValueError as error:
|
||||
raise ControlMessageError("неизвестный тип прикладного сообщения") from error
|
||||
context = MessageContext(ground, rover, epoch, sequence)
|
||||
body = payload[COMMON_SIZE:]
|
||||
if message_type is MessageType.SESSION_HELLO:
|
||||
intent, event = _unpack_exact(body, "!BQ")
|
||||
message: ControlMessage = SessionHello(context, _decode_bool(intent, "emergency_intent"), event)
|
||||
elif message_type is MessageType.SESSION_STATUS:
|
||||
state, latched, event = _unpack_exact(body, "!BBQ")
|
||||
try:
|
||||
safety_state = RoverSafetyCode(state)
|
||||
except ValueError as error:
|
||||
raise ControlMessageError("неизвестное состояние безопасности ровера") from error
|
||||
message = SessionStatus(context, safety_state, _decode_bool(latched, "emergency_latched"), event)
|
||||
elif message_type is MessageType.MOVEMENT_AUTHORIZE_REQUEST:
|
||||
(authorization,) = _unpack_exact(body, "!Q")
|
||||
message = MovementAuthorizeRequest(context, authorization)
|
||||
elif message_type is MessageType.MOVEMENT_AUTHORIZE_ACK:
|
||||
authorization, accepted = _unpack_exact(body, "!QB")
|
||||
message = MovementAuthorizeAck(context, authorization, _decode_bool(accepted, "accepted"))
|
||||
elif message_type is MessageType.CONTROL_STATE:
|
||||
speed, turn, braking, permitted = _unpack_exact(body, "!ffBB")
|
||||
message = ControlStateMessage(
|
||||
context,
|
||||
speed,
|
||||
turn,
|
||||
_decode_bool(braking, "braking"),
|
||||
_decode_bool(permitted, "movement_permitted"),
|
||||
)
|
||||
elif message_type is MessageType.EMERGENCY_STOP:
|
||||
(event,) = _unpack_exact(body, "!Q")
|
||||
message = EmergencyStop(context, event)
|
||||
elif message_type is MessageType.EMERGENCY_ACK:
|
||||
(event,) = _unpack_exact(body, "!Q")
|
||||
message = EmergencyAck(context, event)
|
||||
elif message_type is MessageType.RESET_REQUEST:
|
||||
event, request, speed, permitted = _unpack_exact(body, "!QQfB")
|
||||
message = ResetRequest(context, event, request, speed, _decode_bool(permitted, "movement_permitted"))
|
||||
elif message_type is MessageType.RESET_ACK:
|
||||
event, request, accepted, reason = _unpack_exact(body, "!QQBB")
|
||||
try:
|
||||
reset_reason = SessionRejectReason(reason)
|
||||
except ValueError as error:
|
||||
raise ControlMessageError("неизвестная причина подтверждения сброса") from error
|
||||
message = ResetAck(context, event, request, _decode_bool(accepted, "accepted"), reset_reason)
|
||||
else:
|
||||
rejected_type, reason = _unpack_exact(body, "!BB")
|
||||
try:
|
||||
message = SessionReject(context, MessageType(rejected_type), SessionRejectReason(reason))
|
||||
except ValueError as error:
|
||||
raise ControlMessageError("неизвестный код отказа сеанса") from error
|
||||
return decode_validated_message(message)
|
||||
|
||||
|
||||
def decode_validated_message(message: ControlMessage) -> ControlMessage:
|
||||
"""Повторно применить смысловую проверку к разобранному объекту."""
|
||||
|
||||
return decode_without_recursion(encode_message(message))
|
||||
|
||||
|
||||
def decode_without_recursion(payload: bytes) -> ControlMessage:
|
||||
"""Внутренний разбор уже проверенного представления."""
|
||||
|
||||
version, raw_type, ground, rover, epoch, sequence = struct.unpack(COMMON_FORMAT, payload[:COMMON_SIZE])
|
||||
assert version == MESSAGE_VERSION
|
||||
message_type = MessageType(raw_type)
|
||||
context = MessageContext(ground, rover, epoch, sequence)
|
||||
body = payload[COMMON_SIZE:]
|
||||
if message_type is MessageType.SESSION_HELLO:
|
||||
intent, event = struct.unpack("!BQ", body)
|
||||
return SessionHello(context, bool(intent), event)
|
||||
if message_type is MessageType.SESSION_STATUS:
|
||||
state, latched, event = struct.unpack("!BBQ", body)
|
||||
return SessionStatus(context, RoverSafetyCode(state), bool(latched), event)
|
||||
if message_type is MessageType.MOVEMENT_AUTHORIZE_REQUEST:
|
||||
return MovementAuthorizeRequest(context, struct.unpack("!Q", body)[0])
|
||||
if message_type is MessageType.MOVEMENT_AUTHORIZE_ACK:
|
||||
authorization, accepted = struct.unpack("!QB", body)
|
||||
return MovementAuthorizeAck(context, authorization, bool(accepted))
|
||||
if message_type is MessageType.CONTROL_STATE:
|
||||
speed, turn, braking, permitted = struct.unpack("!ffBB", body)
|
||||
return ControlStateMessage(context, speed, turn, bool(braking), bool(permitted))
|
||||
if message_type is MessageType.EMERGENCY_STOP:
|
||||
return EmergencyStop(context, struct.unpack("!Q", body)[0])
|
||||
if message_type is MessageType.EMERGENCY_ACK:
|
||||
return EmergencyAck(context, struct.unpack("!Q", body)[0])
|
||||
if message_type is MessageType.RESET_REQUEST:
|
||||
event, request, speed, permitted = struct.unpack("!QQfB", body)
|
||||
return ResetRequest(context, event, request, speed, bool(permitted))
|
||||
if message_type is MessageType.RESET_ACK:
|
||||
event, request, accepted, reason = struct.unpack("!QQBB", body)
|
||||
return ResetAck(context, event, request, bool(accepted), SessionRejectReason(reason))
|
||||
rejected_type, reason = struct.unpack("!BB", body)
|
||||
return SessionReject(context, MessageType(rejected_type), SessionRejectReason(reason))
|
||||
|
||||
|
||||
assert COMMON_SIZE == 26
|
||||
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