478 lines
17 KiB
C++
478 lines
17 KiB
C++
#include "cvmmap_streamer/ipc/contracts.hpp"
|
|
|
|
#include <cvmmap/parser.hpp>
|
|
|
|
#include <algorithm>
|
|
#include <limits>
|
|
|
|
namespace cvmmap_streamer::ipc {
|
|
|
|
namespace {
|
|
|
|
constexpr std::size_t kSyncMessageSize = 48;
|
|
constexpr std::size_t kModuleStatusMessageSize = 32;
|
|
constexpr std::size_t kControlRequestBaseSize = 36;
|
|
constexpr std::size_t kControlResponseBaseSize = 40;
|
|
constexpr std::size_t kFrameMetadataRequiredBytes = 36;
|
|
|
|
constexpr std::size_t kSyncFrameCountOffset = 4;
|
|
constexpr std::size_t kSyncTimestampOffset = 16;
|
|
constexpr std::size_t kSyncLabelOffset = 24;
|
|
|
|
constexpr std::size_t kModuleStatusCodeOffset = 4;
|
|
constexpr std::size_t kModuleStatusLabelOffset = 8;
|
|
|
|
constexpr std::size_t kControlCommandOffset = 4;
|
|
constexpr std::size_t kControlReqLabelOffset = 8;
|
|
constexpr std::size_t kControlReqLengthOffset = 32;
|
|
constexpr std::size_t kControlReqPayloadOffset = 34;
|
|
|
|
constexpr std::size_t kControlRespCodeOffset = 8;
|
|
constexpr std::size_t kControlRespLabelOffset = 12;
|
|
constexpr std::size_t kControlRespLengthOffset = 36;
|
|
constexpr std::size_t kControlRespPayloadOffset = 38;
|
|
|
|
constexpr std::size_t kMetaVersionMajorOffset = 8;
|
|
constexpr std::size_t kMetaVersionMinorOffset = 9;
|
|
constexpr std::size_t kMetaFrameCountOffset = 12;
|
|
constexpr std::size_t kMetaTimestampOffset = 16;
|
|
constexpr std::size_t kMetaFrameInfoOffset = 24;
|
|
|
|
constexpr std::size_t kFrameInfoWidthOffset = 0;
|
|
constexpr std::size_t kFrameInfoHeightOffset = 2;
|
|
constexpr std::size_t kFrameInfoChannelsOffset = 4;
|
|
constexpr std::size_t kFrameInfoDepthOffset = 5;
|
|
constexpr std::size_t kFrameInfoPixelFmtOffset = 6;
|
|
constexpr std::size_t kFrameInfoBufferSizeOffset = 8;
|
|
|
|
[[nodiscard]]
|
|
std::uint16_t read_u16_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
|
|
return static_cast<std::uint16_t>(bytes[offset]) |
|
|
(static_cast<std::uint16_t>(bytes[offset + 1]) << 8);
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::uint32_t read_u32_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
|
|
return static_cast<std::uint32_t>(bytes[offset]) |
|
|
(static_cast<std::uint32_t>(bytes[offset + 1]) << 8) |
|
|
(static_cast<std::uint32_t>(bytes[offset + 2]) << 16) |
|
|
(static_cast<std::uint32_t>(bytes[offset + 3]) << 24);
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::int32_t read_i32_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
|
|
return static_cast<std::int32_t>(read_u32_le(bytes, offset));
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::uint64_t read_u64_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
|
|
return static_cast<std::uint64_t>(bytes[offset]) |
|
|
(static_cast<std::uint64_t>(bytes[offset + 1]) << 8) |
|
|
(static_cast<std::uint64_t>(bytes[offset + 2]) << 16) |
|
|
(static_cast<std::uint64_t>(bytes[offset + 3]) << 24) |
|
|
(static_cast<std::uint64_t>(bytes[offset + 4]) << 32) |
|
|
(static_cast<std::uint64_t>(bytes[offset + 5]) << 40) |
|
|
(static_cast<std::uint64_t>(bytes[offset + 6]) << 48) |
|
|
(static_cast<std::uint64_t>(bytes[offset + 7]) << 56);
|
|
}
|
|
|
|
[[nodiscard]]
|
|
bool is_supported_version(std::uint8_t major, std::uint8_t minor) {
|
|
return major == kVersionMajor && minor == kVersionMinor;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::expected<Depth, ParseError> validate_depth(std::uint8_t depth_raw) {
|
|
if (depth_raw > static_cast<std::uint8_t>(Depth::F16)) {
|
|
return std::unexpected(ParseError::InvalidDepth);
|
|
}
|
|
return static_cast<Depth>(depth_raw);
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::expected<PixelFormat, ParseError> validate_pixel_format(std::uint8_t pixel_format_raw) {
|
|
if (pixel_format_raw > static_cast<std::uint8_t>(PixelFormat::YUYV)) {
|
|
return std::unexpected(ParseError::InvalidPixelFormat);
|
|
}
|
|
return static_cast<PixelFormat>(pixel_format_raw);
|
|
}
|
|
|
|
[[nodiscard]]
|
|
DepthUnit from_core_depth_unit(const cvmmap::DepthUnit unit) {
|
|
switch (unit) {
|
|
case cvmmap::DepthUnit::Millimeter:
|
|
return DepthUnit::Millimeter;
|
|
case cvmmap::DepthUnit::Meter:
|
|
return DepthUnit::Meter;
|
|
case cvmmap::DepthUnit::Unknown:
|
|
default:
|
|
return DepthUnit::Unknown;
|
|
}
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::expected<ModuleStatus, ParseError> validate_module_status(std::int32_t status_raw) {
|
|
switch (status_raw) {
|
|
case static_cast<std::int32_t>(ModuleStatus::Online):
|
|
return ModuleStatus::Online;
|
|
case static_cast<std::int32_t>(ModuleStatus::Offline):
|
|
return ModuleStatus::Offline;
|
|
case static_cast<std::int32_t>(ModuleStatus::StreamReset):
|
|
return ModuleStatus::StreamReset;
|
|
default:
|
|
return std::unexpected(ParseError::InvalidModuleStatus);
|
|
}
|
|
}
|
|
|
|
[[nodiscard]]
|
|
ParseError map_core_parser_error(const std::string_view error) {
|
|
if (error.contains("magic")) {
|
|
return ParseError::InvalidMagic;
|
|
}
|
|
if (error.contains("version")) {
|
|
return ParseError::UnsupportedVersion;
|
|
}
|
|
if (error.contains("depth_unit")) {
|
|
return ParseError::InvalidDepthUnit;
|
|
}
|
|
if (error.contains("depth")) {
|
|
return ParseError::InvalidDepth;
|
|
}
|
|
if (error.contains("pixel_format")) {
|
|
return ParseError::InvalidPixelFormat;
|
|
}
|
|
return ParseError::InvalidSize;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
FrameMetadata from_core_metadata(const cvmmap::frame_metadata_t &metadata) {
|
|
FrameMetadata converted{};
|
|
std::copy_n(metadata.magic, converted.magic.size(), converted.magic.begin());
|
|
converted.versions_major = metadata.versions_major;
|
|
converted.versions_minor = metadata.versions_minor;
|
|
converted.frame_count = metadata.frame_count;
|
|
converted.timestamp_ns = metadata.timestamp_ns;
|
|
converted.info.width = metadata.info.width;
|
|
converted.info.height = metadata.info.height;
|
|
converted.info.channels = metadata.info.channels;
|
|
converted.info.depth = static_cast<Depth>(metadata.info.depth);
|
|
converted.info.pixel_format = static_cast<PixelFormat>(metadata.info.pixel_format);
|
|
converted.info.buffer_size = metadata.info.buffer_size;
|
|
return converted;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
FrameInfo from_core_frame_info(const cvmmap::frame_info_t &info) {
|
|
FrameInfo converted{};
|
|
converted.width = info.width;
|
|
converted.height = info.height;
|
|
converted.channels = info.channels;
|
|
converted.depth = static_cast<Depth>(info.depth);
|
|
converted.pixel_format = static_cast<PixelFormat>(info.pixel_format);
|
|
converted.buffer_size = info.buffer_size;
|
|
return converted;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::size_t span_offset(
|
|
const std::span<const std::uint8_t> outer,
|
|
const std::span<const std::uint8_t> inner) {
|
|
if (inner.empty()) {
|
|
return 0;
|
|
}
|
|
return static_cast<std::size_t>(inner.data() - outer.data());
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::size_t payload_bytes_for(
|
|
const std::span<const std::uint8_t> payload_region,
|
|
const cvmmap::parsed_frame_metadata_t &metadata) {
|
|
std::size_t payload_bytes = metadata.left_plane.size();
|
|
const auto consider = [&](std::span<const std::uint8_t> plane) {
|
|
if (plane.empty()) {
|
|
return;
|
|
}
|
|
payload_bytes = std::max(
|
|
payload_bytes,
|
|
span_offset(payload_region, plane) + plane.size());
|
|
};
|
|
consider(metadata.depth_plane);
|
|
consider(metadata.confidence_plane);
|
|
return payload_bytes;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::span<const std::uint8_t> translate_span(
|
|
const std::span<const std::uint8_t> source_payload,
|
|
const std::span<const std::uint8_t> destination_payload,
|
|
const std::span<const std::uint8_t> source_plane) {
|
|
if (source_plane.empty()) {
|
|
return {};
|
|
}
|
|
const auto offset = span_offset(source_payload, source_plane);
|
|
return destination_payload.subspan(offset, source_plane.size());
|
|
}
|
|
|
|
}
|
|
|
|
std::string_view to_string(ParseError error) {
|
|
switch (error) {
|
|
case ParseError::BufferTooSmall:
|
|
return "buffer too small";
|
|
case ParseError::InvalidSize:
|
|
return "invalid message size";
|
|
case ParseError::InvalidMagic:
|
|
return "invalid magic";
|
|
case ParseError::UnsupportedVersion:
|
|
return "unsupported version";
|
|
case ParseError::InvalidDepth:
|
|
return "invalid depth";
|
|
case ParseError::InvalidDepthUnit:
|
|
return "invalid depth unit";
|
|
case ParseError::InvalidPixelFormat:
|
|
return "invalid pixel format";
|
|
case ParseError::InvalidModuleStatus:
|
|
return "invalid module status";
|
|
case ParseError::PayloadLengthMismatch:
|
|
return "payload length mismatch";
|
|
}
|
|
return "unknown parse error";
|
|
}
|
|
|
|
std::string_view to_string(SnapshotError error) {
|
|
switch (error) {
|
|
case SnapshotError::InvalidShmLayout:
|
|
return "invalid shared memory layout";
|
|
case SnapshotError::DestinationTooSmall:
|
|
return "destination buffer too small";
|
|
case SnapshotError::TornRead:
|
|
return "torn read";
|
|
}
|
|
return "unknown snapshot error";
|
|
}
|
|
|
|
std::expected<FrameMetadata, ParseError> parse_frame_metadata(std::span<const std::uint8_t> bytes) {
|
|
if (bytes.size() < kFrameMetadataRequiredBytes) {
|
|
return std::unexpected(ParseError::BufferTooSmall);
|
|
}
|
|
|
|
FrameMetadata metadata{};
|
|
std::copy_n(bytes.begin(), kFrameMetadataMagic.size(), metadata.magic.begin());
|
|
if (metadata.magic != kFrameMetadataMagic) {
|
|
return std::unexpected(ParseError::InvalidMagic);
|
|
}
|
|
|
|
metadata.versions_major = bytes[kMetaVersionMajorOffset];
|
|
metadata.versions_minor = bytes[kMetaVersionMinorOffset];
|
|
if (!is_supported_version(metadata.versions_major, metadata.versions_minor)) {
|
|
return std::unexpected(ParseError::UnsupportedVersion);
|
|
}
|
|
|
|
metadata.frame_count = read_u32_le(bytes, kMetaFrameCountOffset);
|
|
metadata.timestamp_ns = read_u64_le(bytes, kMetaTimestampOffset);
|
|
|
|
auto frame_info_bytes = bytes.subspan(kMetaFrameInfoOffset);
|
|
auto depth = validate_depth(frame_info_bytes[kFrameInfoDepthOffset]);
|
|
if (!depth) {
|
|
return std::unexpected(depth.error());
|
|
}
|
|
auto pixel_format = validate_pixel_format(frame_info_bytes[kFrameInfoPixelFmtOffset]);
|
|
if (!pixel_format) {
|
|
return std::unexpected(pixel_format.error());
|
|
}
|
|
|
|
metadata.info.width = read_u16_le(frame_info_bytes, kFrameInfoWidthOffset);
|
|
metadata.info.height = read_u16_le(frame_info_bytes, kFrameInfoHeightOffset);
|
|
metadata.info.channels = frame_info_bytes[kFrameInfoChannelsOffset];
|
|
metadata.info.depth = *depth;
|
|
metadata.info.pixel_format = *pixel_format;
|
|
metadata.info.buffer_size = read_u32_le(frame_info_bytes, kFrameInfoBufferSizeOffset);
|
|
|
|
return metadata;
|
|
}
|
|
|
|
std::expected<SyncMessage, ParseError> parse_sync_message(std::span<const std::uint8_t> bytes) {
|
|
if (bytes.size() < kSyncMessageSize) {
|
|
return std::unexpected(ParseError::BufferTooSmall);
|
|
}
|
|
if (bytes.size() != kSyncMessageSize) {
|
|
return std::unexpected(ParseError::InvalidSize);
|
|
}
|
|
if (bytes[0] != kFrameTopicMagic) {
|
|
return std::unexpected(ParseError::InvalidMagic);
|
|
}
|
|
if (!is_supported_version(bytes[2], bytes[3])) {
|
|
return std::unexpected(ParseError::UnsupportedVersion);
|
|
}
|
|
|
|
SyncMessage message{};
|
|
message.versions_major = bytes[2];
|
|
message.versions_minor = bytes[3];
|
|
message.frame_count = read_u32_le(bytes, kSyncFrameCountOffset);
|
|
message.timestamp_ns = read_u64_le(bytes, kSyncTimestampOffset);
|
|
std::copy_n(bytes.begin() + kSyncLabelOffset, kLabelLenMax, message.label_bytes.begin());
|
|
|
|
return message;
|
|
}
|
|
|
|
std::expected<ModuleStatusMessage, ParseError> parse_module_status_message(std::span<const std::uint8_t> bytes) {
|
|
if (bytes.size() < kModuleStatusMessageSize) {
|
|
return std::unexpected(ParseError::BufferTooSmall);
|
|
}
|
|
if (bytes.size() != kModuleStatusMessageSize) {
|
|
return std::unexpected(ParseError::InvalidSize);
|
|
}
|
|
if (bytes[0] != kModuleStatusMagic) {
|
|
return std::unexpected(ParseError::InvalidMagic);
|
|
}
|
|
if (!is_supported_version(bytes[2], bytes[3])) {
|
|
return std::unexpected(ParseError::UnsupportedVersion);
|
|
}
|
|
|
|
auto status = validate_module_status(read_i32_le(bytes, kModuleStatusCodeOffset));
|
|
if (!status) {
|
|
return std::unexpected(status.error());
|
|
}
|
|
|
|
ModuleStatusMessage message{};
|
|
message.versions_major = bytes[2];
|
|
message.versions_minor = bytes[3];
|
|
message.module_status = *status;
|
|
std::copy_n(bytes.begin() + kModuleStatusLabelOffset, kLabelLenMax, message.label_bytes.begin());
|
|
|
|
return message;
|
|
}
|
|
|
|
std::expected<ControlRequestMessage, ParseError> parse_control_request_message(std::span<const std::uint8_t> bytes) {
|
|
if (bytes.size() < kControlRequestBaseSize) {
|
|
return std::unexpected(ParseError::BufferTooSmall);
|
|
}
|
|
if (bytes[0] != kControlRequestMagic) {
|
|
return std::unexpected(ParseError::InvalidMagic);
|
|
}
|
|
if (!is_supported_version(bytes[2], bytes[3])) {
|
|
return std::unexpected(ParseError::UnsupportedVersion);
|
|
}
|
|
|
|
const auto payload_size = static_cast<std::size_t>(read_u16_le(bytes, kControlReqLengthOffset));
|
|
if (payload_size > bytes.size() - kControlReqPayloadOffset) {
|
|
return std::unexpected(ParseError::PayloadLengthMismatch);
|
|
}
|
|
if (bytes.size() != kControlRequestBaseSize + payload_size) {
|
|
return std::unexpected(ParseError::InvalidSize);
|
|
}
|
|
|
|
ControlRequestMessage message{};
|
|
message.versions_major = bytes[2];
|
|
message.versions_minor = bytes[3];
|
|
message.command_id = read_i32_le(bytes, kControlCommandOffset);
|
|
std::copy_n(bytes.begin() + kControlReqLabelOffset, kLabelLenMax, message.label_bytes.begin());
|
|
message.request_payload = bytes.subspan(kControlReqPayloadOffset, payload_size);
|
|
|
|
return message;
|
|
}
|
|
|
|
std::expected<ControlResponseMessage, ParseError> parse_control_response_message(std::span<const std::uint8_t> bytes) {
|
|
if (bytes.size() < kControlResponseBaseSize) {
|
|
return std::unexpected(ParseError::BufferTooSmall);
|
|
}
|
|
if (bytes[0] != kControlResponseMagic) {
|
|
return std::unexpected(ParseError::InvalidMagic);
|
|
}
|
|
if (!is_supported_version(bytes[2], bytes[3])) {
|
|
return std::unexpected(ParseError::UnsupportedVersion);
|
|
}
|
|
|
|
const auto payload_size = static_cast<std::size_t>(read_u16_le(bytes, kControlRespLengthOffset));
|
|
if (payload_size > bytes.size() - kControlRespPayloadOffset) {
|
|
return std::unexpected(ParseError::PayloadLengthMismatch);
|
|
}
|
|
if (bytes.size() != kControlResponseBaseSize + payload_size) {
|
|
return std::unexpected(ParseError::InvalidSize);
|
|
}
|
|
|
|
ControlResponseMessage message{};
|
|
message.versions_major = bytes[2];
|
|
message.versions_minor = bytes[3];
|
|
message.command_id = read_i32_le(bytes, kControlCommandOffset);
|
|
message.response_code = read_i32_le(bytes, kControlRespCodeOffset);
|
|
std::copy_n(bytes.begin() + kControlRespLabelOffset, kLabelLenMax, message.label_bytes.begin());
|
|
message.response_payload = bytes.subspan(kControlRespPayloadOffset, payload_size);
|
|
|
|
return message;
|
|
}
|
|
|
|
std::expected<ValidatedShmView, ParseError> validate_shm_region(std::span<const std::uint8_t> shm_region) {
|
|
if (shm_region.size() < kShmPayloadOffset) {
|
|
return std::unexpected(ParseError::BufferTooSmall);
|
|
}
|
|
|
|
auto metadata_result = cvmmap::parse_frame_metadata_regions(
|
|
shm_region.first(kShmPayloadOffset),
|
|
shm_region.subspan(kShmPayloadOffset));
|
|
if (!metadata_result) {
|
|
return std::unexpected(map_core_parser_error(metadata_result.error()));
|
|
}
|
|
|
|
const auto payload_region = shm_region.subspan(kShmPayloadOffset);
|
|
const auto payload_bytes = payload_bytes_for(payload_region, *metadata_result);
|
|
|
|
return ValidatedShmView{
|
|
.metadata = from_core_metadata(metadata_result->normalized_metadata),
|
|
.depth_unit = from_core_depth_unit(metadata_result->depth_unit),
|
|
.payload = payload_region.first(payload_bytes),
|
|
.left = metadata_result->left_plane,
|
|
.depth_info = metadata_result->depth_info
|
|
? std::optional<FrameInfo>(from_core_frame_info(*metadata_result->depth_info))
|
|
: std::nullopt,
|
|
.depth = metadata_result->depth_plane,
|
|
.confidence_info = metadata_result->confidence_info
|
|
? std::optional<FrameInfo>(from_core_frame_info(*metadata_result->confidence_info))
|
|
: std::nullopt,
|
|
.confidence = metadata_result->confidence_plane};
|
|
}
|
|
|
|
std::expected<CoherentSnapshot, SnapshotError> read_coherent_snapshot(
|
|
std::span<const std::uint8_t> shm_region,
|
|
std::span<std::uint8_t> destination,
|
|
const SnapshotReadHook &before_second_metadata_read) {
|
|
auto first = validate_shm_region(shm_region);
|
|
if (!first) {
|
|
return std::unexpected(SnapshotError::InvalidShmLayout);
|
|
}
|
|
|
|
if (destination.size() < first->payload.size()) {
|
|
return std::unexpected(SnapshotError::DestinationTooSmall);
|
|
}
|
|
|
|
std::copy(first->payload.begin(), first->payload.end(), destination.begin());
|
|
|
|
if (before_second_metadata_read) {
|
|
before_second_metadata_read();
|
|
}
|
|
|
|
auto second = validate_shm_region(shm_region);
|
|
if (!second) {
|
|
return std::unexpected(SnapshotError::InvalidShmLayout);
|
|
}
|
|
|
|
if (first->metadata.frame_count != second->metadata.frame_count ||
|
|
first->metadata.timestamp_ns != second->metadata.timestamp_ns) {
|
|
return std::unexpected(SnapshotError::TornRead);
|
|
}
|
|
|
|
const auto copied_payload = std::span<const std::uint8_t>(destination.data(), first->payload.size());
|
|
|
|
return CoherentSnapshot{
|
|
.metadata = first->metadata,
|
|
.depth_unit = first->depth_unit,
|
|
.left = translate_span(first->payload, copied_payload, first->left),
|
|
.depth_info = first->depth_info,
|
|
.depth = translate_span(first->payload, copied_payload, first->depth),
|
|
.confidence_info = first->confidence_info,
|
|
.confidence = translate_span(first->payload, copied_payload, first->confidence),
|
|
.bytes_copied = first->payload.size()};
|
|
}
|
|
|
|
}
|