StreamMessageParser.cpp
Go to the documentation of this file.
2 
3 // standard
4 #include <memory>
5 #include <sstream>
6 
7 // libraries
8 #include <XLink/XLinkPublicDefines.h>
9 #include <spdlog/spdlog.h>
10 
11 #include "utility/Logging.hpp"
12 #include "utility/spdlog-fmt.hpp"
13 
14 // project
40 
41 // shared
67 
68 // StreamPacket structure -> || imgframepixels... , serialized_object, object_type, serialized_object_size ||
69 // object_type -> DataType(int), serialized_object_size -> int
70 
71 namespace dai {
72 
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};
74 
75 // Reads int from little endian format
76 inline int readIntLE(uint8_t* data) {
77  return data[0] + data[1] * 256 + data[2] * 256 * 256 + data[3] * 256 * 256 * 256;
78 }
79 
80 template <class T>
81 inline std::shared_ptr<T> parseDatatype(std::uint8_t* metadata, size_t size, std::vector<uint8_t>& data) {
82  auto tmp = std::make_shared<T>();
83 
84  // deserialize
85  utility::deserialize(metadata, size, *tmp);
86  // Move data
87  tmp->data = std::move(data);
88 
89  return tmp;
90 }
91 
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));
95  }
96  const std::uint32_t packetLength = packet->length - endOfPacketMarker.size();
97  const int serializedObjectSize = readIntLE(packet->data + packetLength - 4);
98  const auto objectType = static_cast<DatatypeEnum>(readIntLE(packet->data + packetLength - 8));
99 
100  uint8_t* marker = packet->data + packetLength;
101  if(memcmp(marker, endOfPacketMarker.data(), endOfPacketMarker.size()) != 0) {
102  std::string hex;
103  for(std::uint32_t i = 0; i < endOfPacketMarker.size(); i++) {
104  hex += fmt::format("{:02X}", marker[i]);
105  }
106  logger::warn("StreamMessageParser end-of-packet marker mismatch, got: " + hex);
107  }
108 
109  const auto info = fmt::format(", total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize);
110 
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);
115  }
116  if(static_cast<int>(packetLength) - 8 - serializedObjectSize < 0) {
117  throw std::runtime_error("Bad packet, couldn't parse (data too small)" + info);
118  }
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);
122  }
123  auto* const metadataStart = packet->data + bufferLength;
124 
125  if(metadataStart < packet->data || metadataStart >= packet->data + packetLength) {
126  throw std::runtime_error("Bad packet, couldn't parse (metadata out of bounds)" + info);
127  }
128 
129  return {objectType, serializedObjectSize, bufferLength};
130 }
131 
132 std::shared_ptr<RawBuffer> StreamMessageParser::parseMessage(streamPacketDesc_t* const packet) {
133  DatatypeEnum objectType;
134  size_t serializedObjectSize;
135  size_t bufferLength;
136  std::tie(objectType, serializedObjectSize, bufferLength) = parseHeader(packet);
137  auto* const metadataStart = packet->data + bufferLength;
138 
139  // copy data part
140  std::vector<uint8_t> data(packet->data, packet->data + bufferLength);
141 
142  // Create corresponding object
143  switch(objectType) {
145  return parseDatatype<RawBuffer>(metadataStart, serializedObjectSize, data);
146  break;
147 
149  return parseDatatype<RawImgFrame>(metadataStart, serializedObjectSize, data);
150  break;
151 
153  return parseDatatype<RawEncodedFrame>(metadataStart, serializedObjectSize, data);
154  break;
155 
157  return parseDatatype<RawNNData>(metadataStart, serializedObjectSize, data);
158  break;
159 
161  return parseDatatype<RawImageManipConfig>(metadataStart, serializedObjectSize, data);
162  break;
163 
165  return parseDatatype<RawCameraControl>(metadataStart, serializedObjectSize, data);
166  break;
167 
169  return parseDatatype<RawImgDetections>(metadataStart, serializedObjectSize, data);
170  break;
171 
173  return parseDatatype<RawSpatialImgDetections>(metadataStart, serializedObjectSize, data);
174  break;
175 
177  return parseDatatype<RawSystemInformation>(metadataStart, serializedObjectSize, data);
178  break;
179 
181  return parseDatatype<RawSpatialLocations>(metadataStart, serializedObjectSize, data);
182  break;
183 
185  return parseDatatype<RawSpatialLocationCalculatorConfig>(metadataStart, serializedObjectSize, data);
186  break;
187 
189  return parseDatatype<RawAprilTags>(metadataStart, serializedObjectSize, data);
190  break;
191 
193  return parseDatatype<RawAprilTagConfig>(metadataStart, serializedObjectSize, data);
194  break;
195 
197  return parseDatatype<RawTracklets>(metadataStart, serializedObjectSize, data);
198  break;
199 
201  return parseDatatype<RawIMUData>(metadataStart, serializedObjectSize, data);
202  break;
203 
205  return parseDatatype<RawStereoDepthConfig>(metadataStart, serializedObjectSize, data);
206  break;
207 
209  return parseDatatype<RawEdgeDetectorConfig>(metadataStart, serializedObjectSize, data);
210  break;
211 
213  return parseDatatype<RawTrackedFeatures>(metadataStart, serializedObjectSize, data);
214  break;
215 
217  return parseDatatype<RawFeatureTrackerConfig>(metadataStart, serializedObjectSize, data);
218  break;
219 
221  return parseDatatype<RawToFConfig>(metadataStart, serializedObjectSize, data);
222  break;
224  return parseDatatype<RawPointCloudConfig>(metadataStart, serializedObjectSize, data);
225  break;
227  return parseDatatype<RawPointCloudData>(metadataStart, serializedObjectSize, data);
228  break;
230  return parseDatatype<RawMessageGroup>(metadataStart, serializedObjectSize, data);
231  break;
233  return parseDatatype<RawImageAlignConfig>(metadataStart, serializedObjectSize, data);
234  break;
235  }
236 
237  throw std::runtime_error(
238  fmt::format("Bad packet, couldn't parse, total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize));
239 }
240 
241 std::shared_ptr<ADatatype> StreamMessageParser::parseMessageToADatatype(streamPacketDesc_t* const packet, DatatypeEnum& objectType) {
242  size_t serializedObjectSize;
243  size_t bufferLength;
244  std::tie(objectType, serializedObjectSize, bufferLength) = parseHeader(packet);
245  auto* const metadataStart = packet->data + bufferLength;
246 
247  // copy data part
248  std::vector<uint8_t> data(packet->data, packet->data + bufferLength);
249 
250  switch(objectType) {
251  case DatatypeEnum::Buffer: {
252  return std::make_shared<Buffer>(parseDatatype<RawBuffer>(metadataStart, serializedObjectSize, data));
253  } break;
254 
256  return std::make_shared<ImgFrame>(parseDatatype<RawImgFrame>(metadataStart, serializedObjectSize, data));
257  break;
258 
260  return std::make_shared<EncodedFrame>(parseDatatype<RawEncodedFrame>(metadataStart, serializedObjectSize, data));
261  break;
262 
264  return std::make_shared<NNData>(parseDatatype<RawNNData>(metadataStart, serializedObjectSize, data));
265  break;
266 
268  return std::make_shared<ImageManipConfig>(parseDatatype<RawImageManipConfig>(metadataStart, serializedObjectSize, data));
269  break;
270 
272  return std::make_shared<CameraControl>(parseDatatype<RawCameraControl>(metadataStart, serializedObjectSize, data));
273  break;
274 
276  return std::make_shared<ImgDetections>(parseDatatype<RawImgDetections>(metadataStart, serializedObjectSize, data));
277  break;
278 
280  return std::make_shared<SpatialImgDetections>(parseDatatype<RawSpatialImgDetections>(metadataStart, serializedObjectSize, data));
281  break;
282 
284  return std::make_shared<SystemInformation>(parseDatatype<RawSystemInformation>(metadataStart, serializedObjectSize, data));
285  break;
286 
288  return std::make_shared<SpatialLocationCalculatorData>(parseDatatype<RawSpatialLocations>(metadataStart, serializedObjectSize, data));
289  break;
290 
292  return std::make_shared<SpatialLocationCalculatorConfig>(
293  parseDatatype<RawSpatialLocationCalculatorConfig>(metadataStart, serializedObjectSize, data));
294  break;
295 
297  return std::make_shared<AprilTags>(parseDatatype<RawAprilTags>(metadataStart, serializedObjectSize, data));
298  break;
299 
301  return std::make_shared<AprilTagConfig>(parseDatatype<RawAprilTagConfig>(metadataStart, serializedObjectSize, data));
302  break;
303 
305  return std::make_shared<Tracklets>(parseDatatype<RawTracklets>(metadataStart, serializedObjectSize, data));
306  break;
307 
309  return std::make_shared<IMUData>(parseDatatype<RawIMUData>(metadataStart, serializedObjectSize, data));
310  break;
311 
313  return std::make_shared<StereoDepthConfig>(parseDatatype<RawStereoDepthConfig>(metadataStart, serializedObjectSize, data));
314  break;
315 
317  return std::make_shared<EdgeDetectorConfig>(parseDatatype<RawEdgeDetectorConfig>(metadataStart, serializedObjectSize, data));
318  break;
319 
321  return std::make_shared<TrackedFeatures>(parseDatatype<RawTrackedFeatures>(metadataStart, serializedObjectSize, data));
322  break;
323 
325  return std::make_shared<FeatureTrackerConfig>(parseDatatype<RawFeatureTrackerConfig>(metadataStart, serializedObjectSize, data));
326  break;
327 
329  return std::make_shared<ToFConfig>(parseDatatype<RawToFConfig>(metadataStart, serializedObjectSize, data));
330  break;
332  return std::make_shared<PointCloudConfig>(parseDatatype<RawPointCloudConfig>(metadataStart, serializedObjectSize, data));
333  break;
335  return std::make_shared<PointCloudData>(parseDatatype<RawPointCloudData>(metadataStart, serializedObjectSize, data));
336  break;
338  return std::make_shared<MessageGroup>(parseDatatype<RawMessageGroup>(metadataStart, serializedObjectSize, data));
339  break;
341  return std::make_shared<ImageAlignConfig>(parseDatatype<RawImageAlignConfig>(metadataStart, serializedObjectSize, data));
342  break;
343  }
344 
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));
347 }
348 
349 std::shared_ptr<ADatatype> StreamMessageParser::parseMessageToADatatype(streamPacketDesc_t* const packet) {
350  DatatypeEnum objectType;
351  return parseMessageToADatatype(packet, objectType);
352 }
353 
354 std::vector<std::uint8_t> StreamMessageParser::serializeMessage(const RawBuffer& data) {
355  // Serialization:
356  // 1. fill vector with bytes from data.data
357  // 2. serialize and append metadata
358  // 3. append datatype enum (4B LE)
359  // 4. append size (4B LE) of serialized metadata
360  // 5. append 16-byte marker/canary
361 
362  DatatypeEnum datatype;
363  std::vector<std::uint8_t> metadata;
364  data.serialize(metadata, datatype);
365  uint32_t metadataSize = static_cast<uint32_t>(metadata.size());
366 
367  // 4B datatype & 4B 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;
372 
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());
379  ser.insert(ser.end(), endOfPacketMarker.begin(), endOfPacketMarker.end());
380 
381  return ser;
382 }
383 
384 std::vector<std::uint8_t> StreamMessageParser::serializeMessage(const std::shared_ptr<const RawBuffer>& data) {
385  if(!data) return {};
386  return serializeMessage(*data);
387 }
388 
389 std::vector<std::uint8_t> StreamMessageParser::serializeMessage(const ADatatype& data) {
390  return serializeMessage(data.serialize());
391 }
392 
393 std::vector<std::uint8_t> StreamMessageParser::serializeMessage(const std::shared_ptr<const ADatatype>& data) {
394  if(!data) return {};
395  return serializeMessage(*data);
396 }
397 
398 } // namespace dai
MessageGroup.hpp
dai::DatatypeEnum::AprilTagConfig
@ AprilTagConfig
dai::StreamMessageParser::parseMessageToADatatype
static std::shared_ptr< ADatatype > parseMessageToADatatype(streamPacketDesc_t *const packet)
Definition: StreamMessageParser.cpp:349
dai::DatatypeEnum::ImageManipConfig
@ ImageManipConfig
dai::DatatypeEnum
DatatypeEnum
Definition: DatatypeEnum.hpp:7
AprilTagConfig.hpp
dai::DatatypeEnum::SpatialLocationCalculatorConfig
@ SpatialLocationCalculatorConfig
RawEncodedFrame.hpp
SystemInformation.hpp
RawToFConfig.hpp
NNData.hpp
ImgFrame.hpp
dai::parseHeader
static std::tuple< DatatypeEnum, size_t, size_t > parseHeader(streamPacketDesc_t *const packet)
Definition: StreamMessageParser.cpp:92
dai::ADatatype
Abstract message.
Definition: ADatatype.hpp:11
RawImageManipConfig.hpp
dai::logger::info
void info(const FormatString &fmt, Args &&...args)
Definition: Logging.hpp:78
dai::DatatypeEnum::Buffer
@ Buffer
RawPointCloudConfig.hpp
dai::DatatypeEnum::SpatialLocationCalculatorData
@ SpatialLocationCalculatorData
dai::DatatypeEnum::PointCloudConfig
@ PointCloudConfig
RawStereoDepthConfig.hpp
RawFeatureTrackerConfig.hpp
EdgeDetectorConfig.hpp
dai::StreamMessageParser::serializeMessage
static std::vector< std::uint8_t > serializeMessage(const std::shared_ptr< const RawBuffer > &data)
Definition: StreamMessageParser.cpp:384
dai::DatatypeEnum::AprilTags
@ AprilTags
RawPointCloudData.hpp
dai::logger::warn
void warn(const FormatString &fmt, Args &&...args)
Definition: Logging.hpp:84
dai::parseDatatype
std::shared_ptr< T > parseDatatype(std::uint8_t *metadata, size_t size, std::vector< uint8_t > &data)
Definition: StreamMessageParser.cpp:81
dai::DatatypeEnum::PointCloudData
@ PointCloudData
DatatypeEnum.hpp
DAI_SPAN_NAMESPACE_NAME::detail::data
constexpr auto data(C &c) -> decltype(c.data())
Definition: span.hpp:177
RawIMUData.hpp
SpatialImgDetections.hpp
ADatatype.hpp
ImgDetections.hpp
RawMessageGroup.hpp
ImageAlignConfig.hpp
dai::DatatypeEnum::EncodedFrame
@ EncodedFrame
RawImgFrame.hpp
dai::utility::deserialize
bool deserialize(const std::uint8_t *data, std::size_t size, T &obj)
Definition: Serialization.hpp:44
SpatialLocationCalculatorData.hpp
DAI_SPAN_NAMESPACE_NAME::detail::size
constexpr auto size(const C &c) -> decltype(c.size())
Definition: span.hpp:167
dai::readIntLE
int readIntLE(uint8_t *data)
Definition: StreamMessageParser.cpp:76
RawEdgeDetectorConfig.hpp
RawBuffer.hpp
RawSpatialLocationCalculatorConfig.hpp
dai::DatatypeEnum::StereoDepthConfig
@ StereoDepthConfig
Tracklets.hpp
StreamMessageParser.hpp
RawAprilTags.hpp
RawTracklets.hpp
RawNNData.hpp
FeatureTrackerConfig.hpp
dai::DatatypeEnum::ImageAlignConfig
@ ImageAlignConfig
dai::DatatypeEnum::TrackedFeatures
@ TrackedFeatures
dai::DatatypeEnum::MessageGroup
@ MessageGroup
RawAprilTagConfig.hpp
PointCloudConfig.hpp
dai::DatatypeEnum::Tracklets
@ Tracklets
RawSystemInformation.hpp
Serialization.hpp
StereoDepthConfig.hpp
dai::endOfPacketMarker
static constexpr std::array< uint8_t, 16 > endOfPacketMarker
Definition: StreamMessageParser.cpp:73
IMUData.hpp
CameraControl.hpp
dai::DatatypeEnum::ToFConfig
@ ToFConfig
RawImgDetections.hpp
dai::DatatypeEnum::CameraControl
@ CameraControl
dai::DatatypeEnum::NNData
@ NNData
dai::DatatypeEnum::SystemInformation
@ SystemInformation
ImageManipConfig.hpp
dai::DatatypeEnum::ImgDetections
@ ImgDetections
RawImageAlignConfig.hpp
dai::RawBuffer
RawBuffer structure.
Definition: RawBuffer.hpp:12
spdlog-fmt.hpp
dai::StreamMessageParser::parseMessage
static std::shared_ptr< RawBuffer > parseMessage(streamPacketDesc_t *const packet)
Definition: StreamMessageParser.cpp:132
dai::DatatypeEnum::EdgeDetectorConfig
@ EdgeDetectorConfig
AprilTags.hpp
ToFConfig.hpp
RawSpatialLocations.hpp
dai::DatatypeEnum::ImgFrame
@ ImgFrame
EncodedFrame.hpp
PointCloudData.hpp
RawCameraControl.hpp
Logging.hpp
dai::DatatypeEnum::IMUData
@ IMUData
Buffer.hpp
dai
Definition: CameraExposureOffset.hpp:6
dai::DatatypeEnum::SpatialImgDetections
@ SpatialImgDetections
TrackedFeatures.hpp
SpatialLocationCalculatorConfig.hpp
RawSpatialImgDetections.hpp
dai::DatatypeEnum::FeatureTrackerConfig
@ FeatureTrackerConfig


depthai
Author(s): Martin Peterlin
autogenerated on Sat Mar 22 2025 02:58:19