435 lines
18 KiB
Python
435 lines
18 KiB
Python
"""Прикладные сообщения безопасного сеанса 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
|