8 #include <XLink/XLinkPublicDefines.h>
9 #include <spdlog/spdlog.h>
73 static constexpr std::array<uint8_t, 16>
endOfPacketMarker = {0xAB, 0xCD, 0xEF, 0x01, 0x23, 0x45, 0x67, 0x89, 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0xDE, 0xF0};
77 return data[0] +
data[1] * 256 +
data[2] * 256 * 256 +
data[3] * 256 * 256 * 256;
82 auto tmp = std::make_shared<T>();
87 tmp->data = std::move(
data);
92 static std::tuple<DatatypeEnum, size_t, size_t>
parseHeader(streamPacketDesc_t*
const packet) {
93 if(packet->length < 24) {
94 throw std::runtime_error(fmt::format(
"Bad packet, couldn't parse (not enough data), total size {}", packet->length));
97 const int serializedObjectSize =
readIntLE(packet->data + packetLength - 4);
100 uint8_t* marker = packet->data + packetLength;
104 hex += fmt::format(
"{:02X}", marker[i]);
106 logger::warn(
"StreamMessageParser end-of-packet marker mismatch, got: " + hex);
109 const auto info = fmt::format(
", total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize);
111 if(serializedObjectSize < 0) {
112 throw std::runtime_error(
"Bad packet, couldn't parse (metadata size negative)" +
info);
113 }
else if(serializedObjectSize >
static_cast<int>(packetLength)) {
114 throw std::runtime_error(
"Bad packet, couldn't parse (metadata size larger than packet length)" +
info);
116 if(
static_cast<int>(packetLength) - 8 - serializedObjectSize < 0) {
117 throw std::runtime_error(
"Bad packet, couldn't parse (data too small)" +
info);
119 const std::uint32_t bufferLength = packetLength - 8 - serializedObjectSize;
120 if(bufferLength > packetLength) {
121 throw std::runtime_error(
"Bad packet, couldn't parse (data too large)" +
info);
123 auto*
const metadataStart = packet->data + bufferLength;
125 if(metadataStart < packet->
data || metadataStart >= packet->data + packetLength) {
126 throw std::runtime_error(
"Bad packet, couldn't parse (metadata out of bounds)" +
info);
129 return {objectType, serializedObjectSize, bufferLength};
134 size_t serializedObjectSize;
136 std::tie(objectType, serializedObjectSize, bufferLength) =
parseHeader(packet);
137 auto*
const metadataStart = packet->data + bufferLength;
140 std::vector<uint8_t>
data(packet->data, packet->data + bufferLength);
145 return parseDatatype<RawBuffer>(metadataStart, serializedObjectSize,
data);
149 return parseDatatype<RawImgFrame>(metadataStart, serializedObjectSize,
data);
153 return parseDatatype<RawEncodedFrame>(metadataStart, serializedObjectSize,
data);
157 return parseDatatype<RawNNData>(metadataStart, serializedObjectSize,
data);
161 return parseDatatype<RawImageManipConfig>(metadataStart, serializedObjectSize,
data);
165 return parseDatatype<RawCameraControl>(metadataStart, serializedObjectSize,
data);
169 return parseDatatype<RawImgDetections>(metadataStart, serializedObjectSize,
data);
173 return parseDatatype<RawSpatialImgDetections>(metadataStart, serializedObjectSize,
data);
177 return parseDatatype<RawSystemInformation>(metadataStart, serializedObjectSize,
data);
181 return parseDatatype<RawSpatialLocations>(metadataStart, serializedObjectSize,
data);
185 return parseDatatype<RawSpatialLocationCalculatorConfig>(metadataStart, serializedObjectSize,
data);
189 return parseDatatype<RawAprilTags>(metadataStart, serializedObjectSize,
data);
193 return parseDatatype<RawAprilTagConfig>(metadataStart, serializedObjectSize,
data);
197 return parseDatatype<RawTracklets>(metadataStart, serializedObjectSize,
data);
201 return parseDatatype<RawIMUData>(metadataStart, serializedObjectSize,
data);
205 return parseDatatype<RawStereoDepthConfig>(metadataStart, serializedObjectSize,
data);
209 return parseDatatype<RawEdgeDetectorConfig>(metadataStart, serializedObjectSize,
data);
213 return parseDatatype<RawTrackedFeatures>(metadataStart, serializedObjectSize,
data);
217 return parseDatatype<RawFeatureTrackerConfig>(metadataStart, serializedObjectSize,
data);
221 return parseDatatype<RawToFConfig>(metadataStart, serializedObjectSize,
data);
224 return parseDatatype<RawPointCloudConfig>(metadataStart, serializedObjectSize,
data);
227 return parseDatatype<RawPointCloudData>(metadataStart, serializedObjectSize,
data);
230 return parseDatatype<RawMessageGroup>(metadataStart, serializedObjectSize,
data);
233 return parseDatatype<RawImageAlignConfig>(metadataStart, serializedObjectSize,
data);
237 throw std::runtime_error(
238 fmt::format(
"Bad packet, couldn't parse, total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize));
242 size_t serializedObjectSize;
244 std::tie(objectType, serializedObjectSize, bufferLength) =
parseHeader(packet);
245 auto*
const metadataStart = packet->data + bufferLength;
248 std::vector<uint8_t>
data(packet->data, packet->data + bufferLength);
252 return std::make_shared<Buffer>(parseDatatype<RawBuffer>(metadataStart, serializedObjectSize,
data));
256 return std::make_shared<ImgFrame>(parseDatatype<RawImgFrame>(metadataStart, serializedObjectSize,
data));
260 return std::make_shared<EncodedFrame>(parseDatatype<RawEncodedFrame>(metadataStart, serializedObjectSize,
data));
264 return std::make_shared<NNData>(parseDatatype<RawNNData>(metadataStart, serializedObjectSize,
data));
268 return std::make_shared<ImageManipConfig>(parseDatatype<RawImageManipConfig>(metadataStart, serializedObjectSize,
data));
272 return std::make_shared<CameraControl>(parseDatatype<RawCameraControl>(metadataStart, serializedObjectSize,
data));
276 return std::make_shared<ImgDetections>(parseDatatype<RawImgDetections>(metadataStart, serializedObjectSize,
data));
280 return std::make_shared<SpatialImgDetections>(parseDatatype<RawSpatialImgDetections>(metadataStart, serializedObjectSize,
data));
284 return std::make_shared<SystemInformation>(parseDatatype<RawSystemInformation>(metadataStart, serializedObjectSize,
data));
288 return std::make_shared<SpatialLocationCalculatorData>(parseDatatype<RawSpatialLocations>(metadataStart, serializedObjectSize,
data));
292 return std::make_shared<SpatialLocationCalculatorConfig>(
293 parseDatatype<RawSpatialLocationCalculatorConfig>(metadataStart, serializedObjectSize,
data));
297 return std::make_shared<AprilTags>(parseDatatype<RawAprilTags>(metadataStart, serializedObjectSize,
data));
301 return std::make_shared<AprilTagConfig>(parseDatatype<RawAprilTagConfig>(metadataStart, serializedObjectSize,
data));
305 return std::make_shared<Tracklets>(parseDatatype<RawTracklets>(metadataStart, serializedObjectSize,
data));
309 return std::make_shared<IMUData>(parseDatatype<RawIMUData>(metadataStart, serializedObjectSize,
data));
313 return std::make_shared<StereoDepthConfig>(parseDatatype<RawStereoDepthConfig>(metadataStart, serializedObjectSize,
data));
317 return std::make_shared<EdgeDetectorConfig>(parseDatatype<RawEdgeDetectorConfig>(metadataStart, serializedObjectSize,
data));
321 return std::make_shared<TrackedFeatures>(parseDatatype<RawTrackedFeatures>(metadataStart, serializedObjectSize,
data));
325 return std::make_shared<FeatureTrackerConfig>(parseDatatype<RawFeatureTrackerConfig>(metadataStart, serializedObjectSize,
data));
329 return std::make_shared<ToFConfig>(parseDatatype<RawToFConfig>(metadataStart, serializedObjectSize,
data));
332 return std::make_shared<PointCloudConfig>(parseDatatype<RawPointCloudConfig>(metadataStart, serializedObjectSize,
data));
335 return std::make_shared<PointCloudData>(parseDatatype<RawPointCloudData>(metadataStart, serializedObjectSize,
data));
338 return std::make_shared<MessageGroup>(parseDatatype<RawMessageGroup>(metadataStart, serializedObjectSize,
data));
341 return std::make_shared<ImageAlignConfig>(parseDatatype<RawImageAlignConfig>(metadataStart, serializedObjectSize,
data));
345 throw std::runtime_error(fmt::format(
346 "Bad packet, couldn't parse (invalid message type), total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize));
363 std::vector<std::uint8_t> metadata;
364 data.serialize(metadata, datatype);
365 uint32_t metadataSize =
static_cast<uint32_t
>(metadata.size());
368 std::array<std::uint8_t, 4> leDatatype;
369 std::array<std::uint8_t, 4> leMetadataSize;
370 for(
int i = 0; i < 4; i++) leDatatype[i] = (static_cast<std::int32_t>(datatype) >> (i * 8)) & 0xFF;
371 for(
int i = 0; i < 4; i++) leMetadataSize[i] = (metadataSize >> i * 8) & 0xFF;
373 std::vector<std::uint8_t> ser;
374 ser.reserve(
data.data.size() + metadata.size() + leDatatype.size() + leMetadataSize.size() +
endOfPacketMarker.size());
375 ser.insert(ser.end(),
data.data.begin(),
data.data.end());
376 ser.insert(ser.end(), metadata.begin(), metadata.end());
377 ser.insert(ser.end(), leDatatype.begin(), leDatatype.end());
378 ser.insert(ser.end(), leMetadataSize.begin(), leMetadataSize.end());