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
|
||||
Reference in New Issue
Block a user