Branch data Line data Source code
1 : : /*
2 : : * Copyright (C) 2026 by Thun Lu. All rights reserved.
3 : : * Author: Thun Lu <thun.lu@zohomail.cn>
4 : : * Repo: https://github.com/thun-res/vlink
5 : : * _ __ __ _ __
6 : : * | | / / / / (_) ____ / /__
7 : : * | | / / / / / / / __ \ / //_/
8 : : * | |/ / / /___ / / / / / / / ,<
9 : : * |___/ /_____/ /_/ /_/ /_/ /_/|_|
10 : : *
11 : : * Licensed under the Apache License, Version 2.0 (the "License");
12 : : * you may not use this file except in compliance with the License.
13 : : * You may obtain a copy of the License at
14 : : *
15 : : * http://www.apache.org/licenses/LICENSE-2.0
16 : : *
17 : : * Unless required by applicable law or agreed to in writing, software
18 : : * distributed under the License is distributed on an "AS IS" BASIS,
19 : : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
20 : : * See the License for the specific language governing permissions and
21 : : * limitations under the License.
22 : : */
23 : :
24 : : #include <vlink/base/helpers.h>
25 : : #include <vlink/base/name_detector.h>
26 : : #include <vlink/serializer.h>
27 : : #include <vlink/zerocopy/message_parser.h>
28 : :
29 : : #include <algorithm>
30 : : #include <array>
31 : : #include <charconv>
32 : : #include <cstring>
33 : : #include <limits>
34 : : #include <string_view>
35 : : #include <type_traits>
36 : : #include <utility>
37 : :
38 : : namespace vlink {
39 : :
40 : : namespace zerocopy {
41 : :
42 : : template <typename T>
43 : 22 : static T read_unaligned(const uint8_t* data) noexcept {
44 : 22 : T value{};
45 : 22 : std::memcpy(&value, data, sizeof(value));
46 : :
47 : 22 : return value;
48 : : }
49 : :
50 : 2 : static float bits_to_float(uint32_t bits) noexcept {
51 : 2 : float value = 0.0F;
52 : 2 : std::memcpy(&value, &bits, sizeof(value));
53 : :
54 : 2 : return value;
55 : : }
56 : :
57 : 46 : static bool parse_index(std::string_view path, std::string_view prefix, size_t& index,
58 : : std::string_view& remainder) noexcept {
59 [ + + - + : 46 : if VUNLIKELY (!Helpers::has_startwith(path, prefix) || path.size() <= prefix.size() || path[prefix.size()] != '[') {
+ + + + +
+ ]
60 : 21 : return false;
61 : : }
62 : :
63 : 25 : const size_t close = path.find(']', prefix.size() + 1);
64 : :
65 [ - + ]: 25 : if VUNLIKELY (close == std::string_view::npos) {
66 : 0 : return false;
67 : : }
68 : :
69 : 25 : const char* first = path.data() + prefix.size() + 1;
70 : 25 : const char* last = path.data() + close;
71 : 25 : const auto result = std::from_chars(first, last, index);
72 : :
73 [ + - - + : 25 : if VUNLIKELY (result.ec != std::errc() || result.ptr != last) {
- + ]
74 : 0 : return false;
75 : : }
76 : :
77 : 25 : remainder = path.substr(close + 1);
78 : :
79 [ + + ]: 25 : if (remainder.empty()) {
80 : 18 : return true;
81 : : }
82 : :
83 [ + + ]: 7 : if VUNLIKELY (!Helpers::has_startwith(remainder, ".")) {
84 : 1 : return false;
85 : : }
86 : :
87 : 6 : remainder.remove_prefix(1);
88 : :
89 : 6 : return true;
90 : : }
91 : :
92 : : template <typename T>
93 : 73 : static bool store_number(T value, MessageParser::Value& out) {
94 : : if constexpr (std::is_floating_point_v<T>) {
95 : 13 : out = static_cast<double>(value);
96 : : } else if constexpr (std::is_signed_v<T>) {
97 : 9 : out = static_cast<int64_t>(value);
98 : : } else {
99 : 51 : out = static_cast<uint64_t>(value);
100 : : }
101 : :
102 : 73 : return true;
103 : : }
104 : :
105 : : template <typename T>
106 : 165 : static bool read_number(std::string_view path, std::string_view field, T value, MessageParser::Value& out) {
107 [ + + ]: 165 : if (path != field) {
108 : 123 : return false;
109 : : }
110 : :
111 : 42 : return store_number(value, out);
112 : : }
113 : :
114 : 5 : static bool read_string(std::string_view path, std::string_view field, std::string_view value,
115 : : MessageParser::Value& out) {
116 [ + + ]: 5 : if (path != field) {
117 : 2 : return false;
118 : : }
119 : :
120 [ + - ]: 3 : out = std::string(value);
121 : :
122 : 3 : return true;
123 : : }
124 : :
125 : 1 : static double half_to_double(uint16_t value) noexcept {
126 : 1 : const uint32_t sign = static_cast<uint32_t>(value & 0x8000U) << 16U;
127 : :
128 : 1 : uint32_t exponent = (value >> 10U) & 0x1FU;
129 : 1 : uint32_t mantissa = value & 0x03FFU;
130 : 1 : uint32_t bits = 0;
131 : :
132 [ - + ]: 1 : if (exponent == 0) {
133 [ # # ]: 0 : if (mantissa == 0) {
134 : 0 : bits = sign;
135 : : } else {
136 : 0 : exponent = 113U;
137 : :
138 [ # # ]: 0 : while ((mantissa & 0x0400U) == 0) {
139 : 0 : mantissa <<= 1U;
140 : 0 : --exponent;
141 : : }
142 : :
143 : 0 : bits = sign | (exponent << 23U) | ((mantissa & 0x03FFU) << 13U);
144 : : }
145 [ - + ]: 1 : } else if (exponent == 0x1FU) {
146 : 0 : bits = sign | 0x7F800000U | (mantissa << 13U);
147 : : } else {
148 : 1 : bits = sign | ((exponent + 112U) << 23U) | (mantissa << 13U);
149 : : }
150 : :
151 : 1 : return static_cast<double>(bits_to_float(bits));
152 : : }
153 : :
154 : 41 : static bool header_value(const Header& header, std::string_view path, MessageParser::Value& out) {
155 [ + + ]: 41 : if (path == "header.frame_id") {
156 [ + - ]: 8 : out = std::string(header.frame_id_view());
157 : 8 : return true;
158 : : }
159 : :
160 [ + - + + ]: 33 : if (read_number(path, "header.seq", header.seq, out)) {
161 : 10 : return true;
162 : : }
163 : :
164 [ + - + + ]: 23 : if (read_number(path, "header.reserved", header.reserved, out)) {
165 : 7 : return true;
166 : : }
167 : :
168 [ + - + + ]: 16 : if (read_number(path, "header.time_meas", header.time_meas, out)) {
169 : 8 : return true;
170 : : }
171 : :
172 [ + - + - ]: 8 : if (read_number(path, "header.time_pub", header.time_pub, out)) {
173 : 8 : return true;
174 : : }
175 : :
176 : 0 : return false;
177 : : }
178 : :
179 : 28 : static bool convert_numeric(const MessageParser::Value& value, double& out, bool* precision_loss) noexcept {
180 : 28 : bool loss = false;
181 : :
182 [ + + ]: 28 : if (const auto* signed_number = std::get_if<int64_t>(&value)) {
183 : 5 : constexpr int64_t kMaxExactInteger = INT64_C(1) << std::numeric_limits<double>::digits;
184 [ + + - + ]: 5 : loss = *signed_number > kMaxExactInteger || *signed_number < -kMaxExactInteger;
185 : 5 : out = static_cast<double>(*signed_number);
186 [ + + ]: 23 : } else if (const auto* unsigned_number = std::get_if<uint64_t>(&value)) {
187 : 10 : constexpr uint64_t kMaxExactInteger = UINT64_C(1) << std::numeric_limits<double>::digits;
188 : 10 : loss = *unsigned_number > kMaxExactInteger;
189 : 10 : out = static_cast<double>(*unsigned_number);
190 [ + - ]: 13 : } else if (const auto* double_number = std::get_if<double>(&value)) {
191 : 13 : out = *double_number;
192 : : } else {
193 : 0 : return false;
194 : : }
195 : :
196 [ + + ]: 28 : if (precision_loss != nullptr) {
197 : 2 : *precision_loss = loss;
198 : : }
199 : :
200 : 28 : return true;
201 : : }
202 : :
203 : 101 : static std::string_view normalize_collection(std::string_view collection) noexcept {
204 [ + + + + : 101 : if (collection == "objects" || collection == "points" || collection == "cells" || collection == "elements") {
+ + + + +
+ ]
205 : 25 : return "data";
206 : : }
207 : :
208 : 76 : return collection;
209 : : }
210 : :
211 : 17 : static MessageParser::ValueType point_value_type(uint8_t type, uint8_t size) noexcept {
212 [ - - + + ]: 17 : switch (type) {
213 : 0 : case PointCloud::kInt8Type:
214 : : case PointCloud::kInt16Type:
215 : : case PointCloud::kInt32Type:
216 : : case PointCloud::kInt64Type:
217 : 0 : return MessageParser::ValueType::kInt64;
218 : 0 : case PointCloud::kBoolType:
219 : : case PointCloud::kUint8Type:
220 : : case PointCloud::kUint16Type:
221 : : case PointCloud::kUint32Type:
222 : : case PointCloud::kUint64Type:
223 : 0 : return MessageParser::ValueType::kUInt64;
224 : 7 : case PointCloud::kUnknownType:
225 [ + + ]: 7 : if (size == sizeof(uint8_t)) {
226 : 1 : return MessageParser::ValueType::kUInt64;
227 : : }
228 : :
229 [ + + ]: 6 : if (size == sizeof(int16_t)) {
230 : 1 : return MessageParser::ValueType::kInt64;
231 : : }
232 : :
233 [ + + + + ]: 5 : if (size == sizeof(float) || size == sizeof(double)) {
234 : 2 : return MessageParser::ValueType::kDouble;
235 : : }
236 : :
237 : 3 : return MessageParser::ValueType::kValueUnknown;
238 : 10 : default:
239 : 10 : return MessageParser::ValueType::kDouble;
240 : : }
241 : : }
242 : :
243 : 27 : static size_t hash_point_field(std::string_view name) noexcept {
244 : 27 : size_t hash = sizeof(size_t) == sizeof(uint64_t) ? static_cast<size_t>(UINT64_C(14695981039346656037))
245 : : : static_cast<size_t>(UINT32_C(2166136261));
246 : 27 : const size_t prime = sizeof(size_t) == sizeof(uint64_t) ? static_cast<size_t>(UINT64_C(1099511628211))
247 : : : static_cast<size_t>(UINT32_C(16777619));
248 [ + + ]: 79 : for (const char character : name) {
249 : 52 : hash ^= static_cast<uint8_t>(character);
250 : 52 : hash *= prime;
251 : : }
252 : :
253 : 27 : return hash;
254 : : }
255 : :
256 : 0 : static MessageParser::ValueType tensor_value_type(uint8_t type) noexcept {
257 [ # # # ]: 0 : switch (type) {
258 : 0 : case Tensor::kInt8:
259 : : case Tensor::kInt16:
260 : : case Tensor::kInt32:
261 : : case Tensor::kInt64:
262 : 0 : return MessageParser::ValueType::kInt64;
263 : 0 : case Tensor::kBool:
264 : : case Tensor::kUint8:
265 : : case Tensor::kUint16:
266 : : case Tensor::kUint32:
267 : : case Tensor::kUint64:
268 : 0 : return MessageParser::ValueType::kUInt64;
269 : 0 : default:
270 : 0 : return MessageParser::ValueType::kDouble;
271 : : }
272 : : }
273 : :
274 : 6 : static size_t audio_element_size(uint8_t format) noexcept {
275 [ - - + - : 6 : switch (format) {
- - ]
276 : 0 : case AudioFrame::kFormatPcmU8:
277 : 0 : return sizeof(uint8_t);
278 : 0 : case AudioFrame::kFormatPcmS16:
279 : 0 : return sizeof(int16_t);
280 : 6 : case AudioFrame::kFormatPcmS24:
281 : 6 : return 3;
282 : 0 : case AudioFrame::kFormatPcmS32:
283 : 0 : return sizeof(int32_t);
284 : 0 : case AudioFrame::kFormatPcmF32:
285 : 0 : return sizeof(float);
286 : 0 : default:
287 : 0 : return 0;
288 : : }
289 : : }
290 : :
291 : 8 : MessageParser::Type MessageParser::detect_type(std::string_view serialized_type) noexcept {
292 : : static constexpr std::pair<std::string_view, Type> kTypes[] = {
293 : : {"RawData", Type::kRawData}, {"CameraFrame", Type::kCameraFrame}, {"PointCloud", Type::kPointCloud},
294 : : {"ProxyData", Type::kProxyData}, {"OccupancyGrid", Type::kOccupancyGrid}, {"Tensor", Type::kTensor},
295 : : {"ObjectArray", Type::kObjectArray}, {"AudioFrame", Type::kAudioFrame},
296 : : };
297 : :
298 [ + + ]: 49 : for (const auto& [name, type] : kTypes) {
299 [ + + ]: 47 : if (serialized_type == name) {
300 : 3 : return type;
301 : : }
302 : :
303 [ + + ]: 44 : if (serialized_type.size() <= name.size()) {
304 : 14 : continue;
305 : : }
306 : :
307 : 30 : const size_t offset = serialized_type.size() - name.size();
308 : 30 : const char separator = serialized_type[offset - 1];
309 : :
310 [ + - + + : 30 : if ((separator == '.' || separator == ':' || separator == '/') && serialized_type.substr(offset) == name) {
+ + + + +
+ ]
311 : 3 : return type;
312 : : }
313 : : }
314 : :
315 : 2 : return Type::kUnknown;
316 : : }
317 : :
318 : 0 : std::string_view MessageParser::type_name(Type type) noexcept {
319 [ # # # # : 0 : switch (type) {
# # # #
# ]
320 : 0 : case Type::kRawData:
321 : 0 : return "RawData";
322 : 0 : case Type::kCameraFrame:
323 : 0 : return "CameraFrame";
324 : 0 : case Type::kPointCloud:
325 : 0 : return "PointCloud";
326 : 0 : case Type::kProxyData:
327 : 0 : return "ProxyData";
328 : 0 : case Type::kOccupancyGrid:
329 : 0 : return "OccupancyGrid";
330 : 0 : case Type::kTensor:
331 : 0 : return "Tensor";
332 : 0 : case Type::kObjectArray:
333 : 0 : return "ObjectArray";
334 : 0 : case Type::kAudioFrame:
335 : 0 : return "AudioFrame";
336 : 0 : default:
337 : 0 : return {};
338 : : }
339 : : }
340 : :
341 : 3 : bool MessageParser::parse(std::string_view serialized_type, const Bytes& bytes) {
342 : 3 : return parse(detect_type(serialized_type), bytes);
343 : : }
344 : :
345 : 35 : bool MessageParser::parse(Type type, const Bytes& bytes) {
346 : 35 : clear();
347 : :
348 : 35 : bool parsed = false;
349 : :
350 [ + + + + : 35 : switch (type) {
+ + + +
- ]
351 : 2 : case Type::kRawData:
352 : 2 : parsed = Serializer::convert(bytes, message_.emplace<RawData>());
353 : 2 : break;
354 : :
355 : 2 : case Type::kCameraFrame:
356 : 2 : parsed = Serializer::convert(bytes, message_.emplace<CameraFrame>());
357 : 2 : break;
358 : :
359 : 6 : case Type::kPointCloud:
360 : 6 : parsed = Serializer::convert(bytes, message_.emplace<PointCloud>());
361 : 6 : break;
362 : :
363 : 2 : case Type::kProxyData:
364 : 2 : parsed = Serializer::convert(bytes, message_.emplace<ProxyData>());
365 : 2 : break;
366 : :
367 : 2 : case Type::kOccupancyGrid:
368 : 2 : parsed = Serializer::convert(bytes, message_.emplace<OccupancyGrid>());
369 : 2 : break;
370 : :
371 : 15 : case Type::kTensor:
372 : 15 : parsed = Serializer::convert(bytes, message_.emplace<Tensor>());
373 : 15 : break;
374 : :
375 : 2 : case Type::kObjectArray:
376 : 2 : parsed = Serializer::convert(bytes, message_.emplace<ObjectArray>());
377 : 2 : break;
378 : :
379 : 4 : case Type::kAudioFrame:
380 : 4 : parsed = Serializer::convert(bytes, message_.emplace<AudioFrame>());
381 : 4 : break;
382 : 0 : default:
383 : 0 : return false;
384 : : }
385 : :
386 [ + + ]: 35 : if VUNLIKELY (!parsed) {
387 : 2 : clear();
388 : 2 : return false;
389 : : }
390 : :
391 [ + + ]: 33 : if (type == Type::kPointCloud) {
392 [ + - ]: 5 : const auto& message = std::get<PointCloud>(message_);
393 : 5 : PointCloud::KeyList keys = message.get_key_list();
394 [ + - ]: 5 : point_fields_.reserve(keys.size());
395 : :
396 : 5 : size_t byte_offset = 0;
397 : :
398 [ + + ]: 22 : for (size_t element_index = 0; element_index < keys.size(); ++element_index) {
399 : 17 : const auto& key = keys[element_index];
400 [ + - ]: 17 : Field field{key.name, point_value_type(key.type, key.size), key.type, key.size};
401 : 17 : field.is_bool = key.type == PointCloud::kBoolType;
402 : 17 : field.byte_offset = byte_offset;
403 : 17 : field.element_index = element_index;
404 [ + - ]: 17 : point_fields_.push_back(std::move(field));
405 : 17 : byte_offset += key.size;
406 : 17 : }
407 : :
408 : 5 : size_t bucket_count = 1;
409 : :
410 [ + + ]: 20 : while (bucket_count < point_fields_.size() * 2) {
411 : 15 : bucket_count <<= 1U;
412 : : }
413 : :
414 : 5 : constexpr auto kNoField = static_cast<size_t>(-1);
415 : :
416 [ + - ]: 5 : point_field_buckets_.assign(bucket_count, kNoField);
417 [ + - ]: 5 : point_field_next_.assign(point_fields_.size(), kNoField);
418 : :
419 [ + + ]: 22 : for (size_t i = point_fields_.size(); i-- > 0;) {
420 : 17 : const size_t bucket = hash_point_field(point_fields_[i].name) & (bucket_count - 1);
421 : 17 : point_field_next_[i] = point_field_buckets_[bucket];
422 : 17 : point_field_buckets_[bucket] = i;
423 : : }
424 : 5 : }
425 : :
426 : 33 : type_ = type;
427 : :
428 [ + + + + : 33 : switch (type_) {
+ + + +
- ]
429 : 2 : case Type::kRawData:
430 : 2 : reserved_[0] = std::get<RawData>(message_).get_reserved();
431 : 2 : break;
432 : 2 : case Type::kCameraFrame:
433 : 2 : reserved_[0] = std::get<CameraFrame>(message_).get_reserved();
434 : 2 : break;
435 : 5 : case Type::kPointCloud:
436 : 5 : reserved_[0] = std::get<PointCloud>(message_).get_reserved();
437 : 5 : reserved_[1] = std::get<PointCloud>(message_).get_reserved2();
438 : 5 : reserved_[2] = std::get<PointCloud>(message_).get_reserved3();
439 : 5 : break;
440 : 2 : case Type::kProxyData:
441 : 2 : reserved_[0] = std::get<ProxyData>(message_).get_reserved();
442 : 2 : reserved_[1] = std::get<ProxyData>(message_).get_reserved2();
443 : 2 : break;
444 : 2 : case Type::kOccupancyGrid:
445 : 2 : reserved_[0] = std::get<OccupancyGrid>(message_).get_reserved();
446 : 2 : reserved_[1] = std::get<OccupancyGrid>(message_).get_reserved2();
447 : 2 : reserved_[2] = std::get<OccupancyGrid>(message_).get_reserved3();
448 : 2 : break;
449 : 15 : case Type::kTensor:
450 : 15 : reserved_[0] = std::get<Tensor>(message_).get_reserved();
451 : 15 : reserved_[1] = std::get<Tensor>(message_).get_reserved2();
452 : 15 : reserved_[2] = std::get<Tensor>(message_).get_reserved3();
453 : 15 : break;
454 : 2 : case Type::kObjectArray:
455 : 2 : reserved_[0] = std::get<ObjectArray>(message_).get_reserved();
456 : 2 : reserved_[1] = std::get<ObjectArray>(message_).get_reserved2();
457 : 2 : reserved_[2] = std::get<ObjectArray>(message_).get_reserved3();
458 : 2 : reserved_[3] = std::get<ObjectArray>(message_).get_reserved4();
459 : 2 : reserved_[4] = std::get<ObjectArray>(message_).get_reserved5();
460 : 2 : break;
461 : 3 : case Type::kAudioFrame:
462 : 3 : reserved_[0] = std::get<AudioFrame>(message_).get_reserved();
463 : 3 : reserved_[1] = std::get<AudioFrame>(message_).get_reserved2();
464 : 3 : break;
465 : 0 : default:
466 : 0 : break;
467 : : }
468 : :
469 : 33 : return true;
470 : : }
471 : :
472 : 38 : void MessageParser::clear() noexcept {
473 : 38 : type_ = Type::kUnknown;
474 : 38 : message_.emplace<std::monostate>();
475 : 38 : reserved_.fill(0);
476 : 38 : point_fields_.clear();
477 : 38 : point_field_buckets_.clear();
478 : 38 : point_field_next_.clear();
479 : 38 : }
480 : :
481 : 80 : bool MessageParser::value(std::string_view path, Value& out) const {
482 [ + + ]: 80 : if (path.find('[') == std::string_view::npos) {
483 [ + - ]: 55 : return root_value(path, out);
484 : : }
485 : :
486 : 25 : size_t index = 0;
487 : 25 : std::string_view field;
488 [ + + ]: 42 : for (const std::string_view collection : {"data", "objects", "points", "cells", "elements", "shape", "strides"}) {
489 [ + + ]: 41 : if (parse_index(path, collection, index, field)) {
490 [ + - ]: 24 : return element_value(normalize_collection(collection), index, field, out);
491 : : }
492 : : }
493 : :
494 [ + - ]: 1 : return root_value(path, out);
495 : : }
496 : :
497 : 14 : bool MessageParser::value(std::string_view collection, size_t index, std::string_view field, Value& out) const {
498 : 14 : return element_value(normalize_collection(collection), index, field, out);
499 : : }
500 : :
501 : 8 : bool MessageParser::value(std::string_view collection, size_t index, const Field& field, Value& out) const {
502 : 8 : collection = normalize_collection(collection);
503 : :
504 [ + + + - : 15 : if (type_ != Type::kPointCloud || collection != "data" || index >= collection_size(collection) ||
+ - - + +
+ ]
505 : 7 : field.element_index >= point_fields_.size()) {
506 : 1 : return false;
507 : : }
508 : :
509 : 7 : const auto& cached = point_fields_[field.element_index];
510 [ + - + - ]: 13 : if (cached.name != field.name || cached.type != field.type || cached.native_type != field.native_type ||
511 [ + + + - : 13 : cached.storage_size != field.storage_size || cached.byte_offset != field.byte_offset) {
- + + + ]
512 : 1 : return false;
513 : : }
514 : :
515 : 6 : return point_value(index, cached, out);
516 : : }
517 : :
518 : 26 : bool MessageParser::numeric(std::string_view path, double& out, bool* precision_loss) const {
519 : 26 : Value parsed;
520 [ + - + + : 52 : return value(path, parsed) && convert_numeric(parsed, out, precision_loss);
+ - ]
521 : 26 : }
522 : :
523 : 5 : bool MessageParser::numeric(std::string_view collection, size_t index, std::string_view field, double& out,
524 : : bool* precision_loss) const {
525 : 5 : Value parsed;
526 [ + - + + : 10 : return value(collection, index, field, parsed) && convert_numeric(parsed, out, precision_loss);
+ - ]
527 : 5 : }
528 : :
529 : 2 : bool MessageParser::numeric(std::string_view collection, size_t index, const Field& field, double& out,
530 : : bool* precision_loss) const {
531 : 2 : Value parsed;
532 [ + - + - : 4 : return value(collection, index, field, parsed) && convert_numeric(parsed, out, precision_loss);
+ - ]
533 : 2 : }
534 : :
535 : 3 : bool MessageParser::text(std::string_view path, std::string& out) const {
536 : 3 : Value parsed;
537 : :
538 [ + - - + ]: 3 : if VUNLIKELY (!value(path, parsed)) {
539 : 0 : return false;
540 : : }
541 : :
542 : 3 : auto* string = std::get_if<std::string>(&parsed);
543 : :
544 [ - + ]: 3 : if VUNLIKELY (string == nullptr) {
545 : 0 : return false;
546 : : }
547 : :
548 : 3 : out = std::move(*string);
549 : :
550 : 3 : return true;
551 : 3 : }
552 : :
553 : 1 : bool MessageParser::text(std::string_view collection, size_t index, std::string_view field, std::string& out) const {
554 : 1 : Value parsed;
555 : :
556 [ + - - + ]: 1 : if VUNLIKELY (!value(collection, index, field, parsed)) {
557 : 0 : return false;
558 : : }
559 : :
560 : 1 : auto* string = std::get_if<std::string>(&parsed);
561 : :
562 [ - + ]: 1 : if VUNLIKELY (string == nullptr) {
563 : 0 : return false;
564 : : }
565 : :
566 : 1 : out = std::move(*string);
567 : :
568 : 1 : return true;
569 : 1 : }
570 : :
571 : 56 : bool MessageParser::root_value(std::string_view path, Value& out) const {
572 [ + + ]: 56 : if (Helpers::has_startwith(path, "header.")) {
573 : 42 : const Header* header = nullptr;
574 [ + + + + : 42 : switch (type_) {
+ + + + ]
575 : 7 : case Type::kRawData:
576 : 7 : header = &get<RawData>()->header;
577 : 7 : break;
578 : :
579 : 9 : case Type::kCameraFrame:
580 : 9 : header = &get<CameraFrame>()->header;
581 : 9 : break;
582 : :
583 : 5 : case Type::kPointCloud:
584 : 5 : header = &get<PointCloud>()->header;
585 : 5 : break;
586 : :
587 : 5 : case Type::kOccupancyGrid:
588 : 5 : header = &get<OccupancyGrid>()->header;
589 : 5 : break;
590 : :
591 : 5 : case Type::kTensor:
592 : 5 : header = &get<Tensor>()->header;
593 : 5 : break;
594 : :
595 : 5 : case Type::kObjectArray:
596 : 5 : header = &get<ObjectArray>()->header;
597 : 5 : break;
598 : :
599 : 5 : case Type::kAudioFrame:
600 : 5 : header = &get<AudioFrame>()->header;
601 : 5 : break;
602 : 1 : default:
603 : 1 : break;
604 : : }
605 : :
606 [ + + + - ]: 42 : return header != nullptr && header_value(*header, path, out);
607 : : }
608 : :
609 [ + + + + : 14 : switch (type_) {
- - - +
+ ]
610 : 1 : case Type::kRawData: {
611 : 1 : const auto& message = *get<RawData>();
612 : :
613 [ + - - + ]: 1 : if (read_number(path, "size", message.size(), out)) {
614 : 0 : return true;
615 : : }
616 : :
617 [ + - - + ]: 1 : if (read_number(path, "reserved", reserved_[0], out)) {
618 : 0 : return true;
619 : : }
620 : :
621 [ + - ]: 1 : if (path == "data") {
622 : 1 : out = Bytes::shallow_copy(message.data(), message.size());
623 : 1 : return true;
624 : : }
625 : :
626 : 0 : break;
627 : : }
628 : :
629 : 8 : case Type::kCameraFrame: {
630 : 8 : const auto& message = *get<CameraFrame>();
631 : :
632 [ + - + + ]: 8 : if (read_number(path, "channel", message.channel(), out)) {
633 : 1 : return true;
634 : : }
635 : :
636 [ + - + + ]: 7 : if (read_number(path, "width", message.width(), out)) {
637 : 2 : return true;
638 : : }
639 : :
640 [ + - + + ]: 5 : if (read_number(path, "height", message.height(), out)) {
641 : 1 : return true;
642 : : }
643 : :
644 [ + - + + ]: 4 : if (read_number(path, "freq", message.freq(), out)) {
645 : 1 : return true;
646 : : }
647 : :
648 [ + - + + ]: 3 : if (read_number(path, "format", message.format(), out)) {
649 : 1 : return true;
650 : : }
651 : :
652 [ + - + + ]: 2 : if (read_number(path, "stream", message.stream(), out)) {
653 : 1 : return true;
654 : : }
655 : :
656 [ + - + - ]: 1 : if (read_number(path, "size", message.size(), out)) {
657 : 1 : return true;
658 : : }
659 : :
660 [ # # # # ]: 0 : if (read_number(path, "reserved", reserved_[0], out)) {
661 : 0 : return true;
662 : : }
663 : :
664 [ # # ]: 0 : if (path == "data") {
665 : 0 : out = Bytes::shallow_copy(message.data(), message.size());
666 : 0 : return true;
667 : : }
668 : :
669 : 0 : break;
670 : : }
671 : :
672 : 1 : case Type::kPointCloud: {
673 : 1 : const auto& message = *get<PointCloud>();
674 : :
675 [ + - - + ]: 1 : if (read_number(path, "size", message.size(), out)) {
676 : 0 : return true;
677 : : }
678 : :
679 [ + - - + ]: 1 : if (read_number(path, "pack_size", message.pack_size(), out)) {
680 : 0 : return true;
681 : : }
682 : :
683 [ + - - + ]: 1 : if (read_number(path, "extent", message.get_extent(), out)) {
684 : 0 : return true;
685 : : }
686 : :
687 [ + - - + ]: 1 : if (read_number(path, "downsample", message.get_downsample(), out)) {
688 : 0 : return true;
689 : : }
690 : :
691 [ + - - + ]: 1 : if (read_number(path, "vertical", message.get_vertical(), out)) {
692 : 0 : return true;
693 : : }
694 : :
695 [ + - - + ]: 1 : if (read_number(path, "reserved_size", message.get_reserved_size(), out)) {
696 : 0 : return true;
697 : : }
698 : :
699 [ + - - + ]: 1 : if (read_number(path, "get_reserved_size", message.get_reserved_size(), out)) {
700 : 0 : return true;
701 : : }
702 : :
703 [ + - - + ]: 1 : if (read_number(path, "reserved", reserved_[0], out)) {
704 : 0 : return true;
705 : : }
706 : :
707 [ + - - + ]: 1 : if (read_number(path, "reserved2", reserved_[1], out)) {
708 : 0 : return true;
709 : : }
710 : :
711 [ + - - + ]: 1 : if (read_number(path, "reserved3", reserved_[2], out)) {
712 : 0 : return true;
713 : : }
714 : :
715 [ - + ]: 1 : if (path == "data") {
716 : 0 : out = Bytes::shallow_copy(message.get_internal_data(), message.size() * message.pack_size());
717 : 0 : return true;
718 : : }
719 : :
720 : 1 : break;
721 : : }
722 : :
723 : 2 : case Type::kProxyData: {
724 : 2 : const auto& message = *get<ProxyData>();
725 : :
726 [ + - - + ]: 2 : if (read_number(path, "control_id", message.control_id(), out)) {
727 : 0 : return true;
728 : : }
729 : :
730 [ + - - + ]: 2 : if (read_number(path, "mode", message.mode(), out)) {
731 : 0 : return true;
732 : : }
733 : :
734 [ + - - + ]: 2 : if (read_number(path, "timestamp", message.timestamp(), out)) {
735 : 0 : return true;
736 : : }
737 : :
738 [ + - - + ]: 2 : if (read_number(path, "seq", message.seq(), out)) {
739 : 0 : return true;
740 : : }
741 : :
742 [ + - - + ]: 2 : if (read_number(path, "schema", message.schema(), out)) {
743 : 0 : return true;
744 : : }
745 : :
746 [ + - - + ]: 2 : if (read_number(path, "size", message.raw().size(), out)) {
747 : 0 : return true;
748 : : }
749 : :
750 [ + - - + ]: 2 : if (read_number(path, "raw.size", message.raw().size(), out)) {
751 : 0 : return true;
752 : : }
753 : :
754 [ + - - + ]: 2 : if (read_number(path, "reserved", reserved_[0], out)) {
755 : 0 : return true;
756 : : }
757 : :
758 [ + - - + ]: 2 : if (read_number(path, "reserved2", reserved_[1], out)) {
759 : 0 : return true;
760 : : }
761 : :
762 [ + - + + ]: 2 : if (read_string(path, "url", message.url(), out)) {
763 : 1 : return true;
764 : : }
765 : :
766 [ + - + - ]: 1 : if (read_string(path, "ser", message.ser(), out)) {
767 : 1 : return true;
768 : : }
769 : :
770 [ # # # # ]: 0 : if (read_string(path, "hostname", message.hostname(), out)) {
771 : 0 : return true;
772 : : }
773 : :
774 [ # # # # : 0 : if (path == "raw" || path == "data") {
# # ]
775 : 0 : out = message.raw();
776 : 0 : return true;
777 : : }
778 : :
779 : 0 : break;
780 : : }
781 : :
782 : 0 : case Type::kOccupancyGrid: {
783 : 0 : const auto& message = *get<OccupancyGrid>();
784 : :
785 [ # # # # ]: 0 : if (read_string(path, "map_id", message.map_id(), out)) {
786 : 0 : return true;
787 : : }
788 : :
789 [ # # # # ]: 0 : if (read_number(path, "channel", message.channel(), out)) {
790 : 0 : return true;
791 : : }
792 : :
793 [ # # # # ]: 0 : if (read_number(path, "freq", message.freq(), out)) {
794 : 0 : return true;
795 : : }
796 : :
797 [ # # # # ]: 0 : if (read_number(path, "width", message.width(), out)) {
798 : 0 : return true;
799 : : }
800 : :
801 [ # # # # ]: 0 : if (read_number(path, "height", message.height(), out)) {
802 : 0 : return true;
803 : : }
804 : :
805 [ # # # # ]: 0 : if (read_number(path, "resolution", message.resolution(), out)) {
806 : 0 : return true;
807 : : }
808 : :
809 [ # # # # ]: 0 : if (read_number(path, "origin_x", message.origin_x(), out)) {
810 : 0 : return true;
811 : : }
812 : :
813 [ # # # # ]: 0 : if (read_number(path, "origin_y", message.origin_y(), out)) {
814 : 0 : return true;
815 : : }
816 : :
817 [ # # # # ]: 0 : if (read_number(path, "origin_z", message.origin_z(), out)) {
818 : 0 : return true;
819 : : }
820 : :
821 [ # # # # ]: 0 : if (read_number(path, "origin_yaw", message.origin_yaw(), out)) {
822 : 0 : return true;
823 : : }
824 : :
825 [ # # # # ]: 0 : if (read_number(path, "cell_type", message.cell_type(), out)) {
826 : 0 : return true;
827 : : }
828 : :
829 [ # # # # ]: 0 : if (read_number(path, "cell_size", message.cell_size(), out)) {
830 : 0 : return true;
831 : : }
832 : :
833 [ # # # # ]: 0 : if (read_number(path, "default_value", message.default_value(), out)) {
834 : 0 : return true;
835 : : }
836 : :
837 [ # # # # ]: 0 : if (read_number(path, "value_min", message.value_min(), out)) {
838 : 0 : return true;
839 : : }
840 : :
841 [ # # # # ]: 0 : if (read_number(path, "value_max", message.value_max(), out)) {
842 : 0 : return true;
843 : : }
844 : :
845 [ # # # # ]: 0 : if (read_number(path, "occupied_threshold", message.occupied_threshold(), out)) {
846 : 0 : return true;
847 : : }
848 : :
849 [ # # # # ]: 0 : if (read_number(path, "free_threshold", message.free_threshold(), out)) {
850 : 0 : return true;
851 : : }
852 : :
853 [ # # # # ]: 0 : if (read_number(path, "valid_cell_count", message.valid_cell_count(), out)) {
854 : 0 : return true;
855 : : }
856 : :
857 [ # # # # ]: 0 : if (read_number(path, "update_time_ns", message.update_time_ns(), out)) {
858 : 0 : return true;
859 : : }
860 : :
861 [ # # # # ]: 0 : if (read_number(path, "size", message.size(), out)) {
862 : 0 : return true;
863 : : }
864 : :
865 [ # # # # ]: 0 : if (read_number(path, "reserved", reserved_[0], out)) {
866 : 0 : return true;
867 : : }
868 : :
869 [ # # # # ]: 0 : if (read_number(path, "reserved2", reserved_[1], out)) {
870 : 0 : return true;
871 : : }
872 : :
873 [ # # # # ]: 0 : if (read_number(path, "reserved3", reserved_[2], out)) {
874 : 0 : return true;
875 : : }
876 : :
877 [ # # ]: 0 : if (path == "data") {
878 : 0 : out = Bytes::shallow_copy(message.data(), message.size());
879 : 0 : return true;
880 : : }
881 : :
882 : 0 : break;
883 : : }
884 : :
885 : 0 : case Type::kTensor: {
886 : 0 : const auto& message = *get<Tensor>();
887 : :
888 [ # # # # ]: 0 : if (read_string(path, "name", message.name(), out)) {
889 : 0 : return true;
890 : : }
891 : :
892 [ # # # # ]: 0 : if (read_string(path, "model_id", message.model_id(), out)) {
893 : 0 : return true;
894 : : }
895 : :
896 [ # # # # ]: 0 : if (read_string(path, "layout", message.layout(), out)) {
897 : 0 : return true;
898 : : }
899 : :
900 [ # # # # ]: 0 : if (read_number(path, "dtype", message.dtype(), out)) {
901 : 0 : return true;
902 : : }
903 : :
904 [ # # # # ]: 0 : if (read_number(path, "device", message.device(), out)) {
905 : 0 : return true;
906 : : }
907 : :
908 [ # # # # ]: 0 : if (read_number(path, "rank", message.rank(), out)) {
909 : 0 : return true;
910 : : }
911 : :
912 [ # # # # ]: 0 : if (read_number(path, "num_elements", message.num_elements(), out)) {
913 : 0 : return true;
914 : : }
915 : :
916 [ # # # # ]: 0 : if (read_number(path, "element_size", message.element_size(), out)) {
917 : 0 : return true;
918 : : }
919 : :
920 [ # # # # ]: 0 : if (read_number(path, "batch_size", message.batch_size(), out)) {
921 : 0 : return true;
922 : : }
923 : :
924 [ # # # # ]: 0 : if (read_number(path, "quant_scale", message.quant_scale(), out)) {
925 : 0 : return true;
926 : : }
927 : :
928 [ # # # # ]: 0 : if (read_number(path, "quant_zero_point", message.quant_zero_point(), out)) {
929 : 0 : return true;
930 : : }
931 : :
932 [ # # # # ]: 0 : if (read_number(path, "channel", message.channel(), out)) {
933 : 0 : return true;
934 : : }
935 : :
936 [ # # # # ]: 0 : if (read_number(path, "freq", message.freq(), out)) {
937 : 0 : return true;
938 : : }
939 : :
940 [ # # # # ]: 0 : if (read_number(path, "update_time_ns", message.update_time_ns(), out)) {
941 : 0 : return true;
942 : : }
943 : :
944 [ # # # # ]: 0 : if (read_number(path, "size", message.size(), out)) {
945 : 0 : return true;
946 : : }
947 : :
948 [ # # # # ]: 0 : if (read_number(path, "reserved", reserved_[0], out)) {
949 : 0 : return true;
950 : : }
951 : :
952 [ # # # # ]: 0 : if (read_number(path, "reserved2", reserved_[1], out)) {
953 : 0 : return true;
954 : : }
955 : :
956 [ # # # # ]: 0 : if (read_number(path, "reserved3", reserved_[2], out)) {
957 : 0 : return true;
958 : : }
959 : :
960 [ # # ]: 0 : if (path == "data") {
961 : 0 : out = Bytes::shallow_copy(message.data(), message.size());
962 : 0 : return true;
963 : : }
964 : :
965 : 0 : break;
966 : : }
967 : :
968 : 0 : case Type::kObjectArray: {
969 : 0 : const auto& message = *get<ObjectArray>();
970 : :
971 [ # # # # ]: 0 : if (read_string(path, "source_id", message.source_id(), out)) {
972 : 0 : return true;
973 : : }
974 : :
975 [ # # # # ]: 0 : if (read_number(path, "channel", message.channel(), out)) {
976 : 0 : return true;
977 : : }
978 : :
979 [ # # # # ]: 0 : if (read_number(path, "freq", message.freq(), out)) {
980 : 0 : return true;
981 : : }
982 : :
983 [ # # # # ]: 0 : if (read_number(path, "count", message.count(), out)) {
984 : 0 : return true;
985 : : }
986 : :
987 [ # # # # ]: 0 : if (read_number(path, "pack_size", message.pack_size(), out)) {
988 : 0 : return true;
989 : : }
990 : :
991 [ # # # # ]: 0 : if (read_number(path, "size", static_cast<uint64_t>(message.count()) * message.pack_size(), out)) {
992 : 0 : return true;
993 : : }
994 : :
995 [ # # # # ]: 0 : if (read_number(path, "update_time_ns", message.update_time_ns(), out)) {
996 : 0 : return true;
997 : : }
998 : :
999 [ # # # # ]: 0 : if (read_number(path, "reserved", reserved_[0], out)) {
1000 : 0 : return true;
1001 : : }
1002 : :
1003 [ # # # # ]: 0 : if (read_number(path, "reserved2", reserved_[1], out)) {
1004 : 0 : return true;
1005 : : }
1006 : :
1007 [ # # # # ]: 0 : if (read_number(path, "reserved3", reserved_[2], out)) {
1008 : 0 : return true;
1009 : : }
1010 : :
1011 [ # # # # ]: 0 : if (read_number(path, "reserved4", reserved_[3], out)) {
1012 : 0 : return true;
1013 : : }
1014 : :
1015 [ # # # # ]: 0 : if (read_number(path, "reserved5", reserved_[4], out)) {
1016 : 0 : return true;
1017 : : }
1018 : :
1019 [ # # ]: 0 : if (path == "data") {
1020 : 0 : out = Bytes::shallow_copy(message.data(), static_cast<size_t>(message.count()) * message.pack_size());
1021 : 0 : return true;
1022 : : }
1023 : :
1024 : 0 : break;
1025 : : }
1026 : :
1027 : 1 : case Type::kAudioFrame: {
1028 : 1 : const auto& message = *get<AudioFrame>();
1029 : :
1030 [ + - - + ]: 1 : if (read_string(path, "codec", message.codec(), out)) {
1031 : 0 : return true;
1032 : : }
1033 : :
1034 [ + - + - ]: 1 : if (read_string(path, "language", message.language(), out)) {
1035 : 1 : return true;
1036 : : }
1037 : :
1038 [ # # # # ]: 0 : if (read_number(path, "channel", message.channel(), out)) {
1039 : 0 : return true;
1040 : : }
1041 : :
1042 [ # # # # ]: 0 : if (read_number(path, "freq", message.freq(), out)) {
1043 : 0 : return true;
1044 : : }
1045 : :
1046 [ # # # # ]: 0 : if (read_number(path, "sample_rate", message.sample_rate(), out)) {
1047 : 0 : return true;
1048 : : }
1049 : :
1050 [ # # # # ]: 0 : if (read_number(path, "num_samples", message.num_samples(), out)) {
1051 : 0 : return true;
1052 : : }
1053 : :
1054 [ # # # # ]: 0 : if (read_number(path, "num_channels", message.num_channels(), out)) {
1055 : 0 : return true;
1056 : : }
1057 : :
1058 [ # # # # ]: 0 : if (read_number(path, "bit_depth", message.bit_depth(), out)) {
1059 : 0 : return true;
1060 : : }
1061 : :
1062 [ # # # # ]: 0 : if (read_number(path, "bitrate", message.bitrate(), out)) {
1063 : 0 : return true;
1064 : : }
1065 : :
1066 [ # # # # ]: 0 : if (read_number(path, "format", message.format(), out)) {
1067 : 0 : return true;
1068 : : }
1069 : :
1070 [ # # # # ]: 0 : if (read_number(path, "layout", message.layout(), out)) {
1071 : 0 : return true;
1072 : : }
1073 : :
1074 [ # # # # ]: 0 : if (read_number(path, "duration_ns", message.duration_ns(), out)) {
1075 : 0 : return true;
1076 : : }
1077 : :
1078 [ # # # # ]: 0 : if (read_number(path, "update_time_ns", message.update_time_ns(), out)) {
1079 : 0 : return true;
1080 : : }
1081 : :
1082 [ # # # # ]: 0 : if (read_number(path, "size", message.size(), out)) {
1083 : 0 : return true;
1084 : : }
1085 : :
1086 [ # # # # ]: 0 : if (read_number(path, "reserved", reserved_[0], out)) {
1087 : 0 : return true;
1088 : : }
1089 : :
1090 [ # # # # ]: 0 : if (read_number(path, "reserved2", reserved_[1], out)) {
1091 : 0 : return true;
1092 : : }
1093 : :
1094 [ # # ]: 0 : if (path == "data") {
1095 : 0 : out = Bytes::shallow_copy(message.data(), message.size());
1096 : 0 : return true;
1097 : : }
1098 : :
1099 : 0 : break;
1100 : : }
1101 : :
1102 : 1 : default:
1103 : 1 : break;
1104 : : }
1105 : :
1106 : 2 : return false;
1107 : : }
1108 : :
1109 : 38 : bool MessageParser::element_value(std::string_view collection, size_t index, std::string_view field, Value& out) const {
1110 [ + + ]: 38 : if VUNLIKELY (index >= collection_size(collection)) {
1111 : 3 : return false;
1112 : : }
1113 : :
1114 [ + + ]: 35 : if (type_ == Type::kTensor) {
1115 : 18 : const auto& message = *get<Tensor>();
1116 : :
1117 [ + + ]: 18 : if (collection == "shape") {
1118 [ + + + - : 3 : if VUNLIKELY (!field.empty() && field != "value") {
+ + ]
1119 : 2 : return false;
1120 : : }
1121 : :
1122 : 1 : return store_number(message.shape_at(static_cast<uint8_t>(index)), out);
1123 : : }
1124 : :
1125 [ - + ]: 15 : if (collection == "strides") {
1126 [ # # # # : 0 : if VUNLIKELY (!field.empty() && field != "value") {
# # ]
1127 : 0 : return false;
1128 : : }
1129 : :
1130 : 0 : return store_number(message.stride_at(static_cast<uint8_t>(index)), out);
1131 : : }
1132 : :
1133 [ + - - + : 15 : if (collection != "data" || (!field.empty() && field != "value")) {
- - - + ]
1134 : 0 : return false;
1135 : : }
1136 : :
1137 : 15 : const uint8_t* data = message.data() + index * message.element_size();
1138 [ + + + + : 15 : switch (message.dtype()) {
+ + + + +
+ + + +
- ]
1139 : 1 : case Tensor::kBool:
1140 : 1 : return store_number(read_unaligned<uint8_t>(data) != 0, out);
1141 : 1 : case Tensor::kInt8:
1142 : 1 : return store_number(read_unaligned<int8_t>(data), out);
1143 : 1 : case Tensor::kUint8:
1144 : 1 : return store_number(read_unaligned<uint8_t>(data), out);
1145 : 1 : case Tensor::kInt16:
1146 : 1 : return store_number(read_unaligned<int16_t>(data), out);
1147 : 1 : case Tensor::kUint16:
1148 : 1 : return store_number(read_unaligned<uint16_t>(data), out);
1149 : 1 : case Tensor::kInt32:
1150 : 1 : return store_number(read_unaligned<int32_t>(data), out);
1151 : 1 : case Tensor::kUint32:
1152 : 1 : return store_number(read_unaligned<uint32_t>(data), out);
1153 : 3 : case Tensor::kInt64:
1154 : 3 : return store_number(read_unaligned<int64_t>(data), out);
1155 : 1 : case Tensor::kUint64:
1156 : 1 : return store_number(read_unaligned<uint64_t>(data), out);
1157 : 1 : case Tensor::kFloat16:
1158 : 1 : out = half_to_double(read_unaligned<uint16_t>(data));
1159 : 1 : return true;
1160 : 1 : case Tensor::kBfloat16: {
1161 : 1 : const uint32_t bits = static_cast<uint32_t>(read_unaligned<uint16_t>(data)) << 16U;
1162 : 1 : out = static_cast<double>(bits_to_float(bits));
1163 : 1 : return true;
1164 : : }
1165 : :
1166 : 1 : case Tensor::kFloat32:
1167 : 1 : return store_number(read_unaligned<float>(data), out);
1168 : 1 : case Tensor::kFloat64:
1169 : 1 : return store_number(read_unaligned<double>(data), out);
1170 : 0 : default:
1171 : 0 : return false;
1172 : : }
1173 : : }
1174 : :
1175 [ + + + - : 17 : if (type_ == Type::kOccupancyGrid && collection == "data") {
+ + ]
1176 : 1 : const auto& message = *get<OccupancyGrid>();
1177 : :
1178 [ - + - - : 1 : if VUNLIKELY (!field.empty() && field != "value") {
- + ]
1179 : 0 : return false;
1180 : : }
1181 : :
1182 : 1 : const uint8_t* data = message.data() + index * message.cell_size();
1183 : :
1184 [ - - + - : 1 : switch (message.cell_type()) {
- ]
1185 : 0 : case OccupancyGrid::kCellInt8:
1186 : 0 : return store_number(read_unaligned<int8_t>(data), out);
1187 : 0 : case OccupancyGrid::kCellUint8:
1188 : 0 : return store_number(read_unaligned<uint8_t>(data), out);
1189 : 1 : case OccupancyGrid::kCellUint16:
1190 : 1 : return store_number(read_unaligned<uint16_t>(data), out);
1191 : 0 : case OccupancyGrid::kCellFloat32:
1192 : 0 : return store_number(read_unaligned<float>(data), out);
1193 : 0 : default:
1194 : 0 : return false;
1195 : : }
1196 : : }
1197 : :
1198 [ + + + - : 16 : if (type_ == Type::kPointCloud && collection == "data") {
+ + ]
1199 [ - + ]: 10 : if VUNLIKELY (point_field_buckets_.empty()) {
1200 : 0 : return false;
1201 : : }
1202 : :
1203 : 10 : constexpr auto kNoField = static_cast<size_t>(-1);
1204 : 10 : size_t field_index = point_field_buckets_[hash_point_field(field) & (point_field_buckets_.size() - 1)];
1205 : :
1206 [ + + - + : 10 : while (field_index != kNoField && point_fields_[field_index].name != field) {
- + ]
1207 : 0 : field_index = point_field_next_[field_index];
1208 : : }
1209 : :
1210 [ + + ]: 10 : if VUNLIKELY (field_index == kNoField) {
1211 : 1 : return false;
1212 : : }
1213 : :
1214 : 9 : return point_value(index, point_fields_[field_index], out);
1215 : : }
1216 : :
1217 [ + + + - : 6 : if (type_ == Type::kAudioFrame && collection == "data") {
+ + ]
1218 : 2 : const auto& message = *get<AudioFrame>();
1219 : :
1220 [ + - - + : 2 : if VUNLIKELY (!field.empty() && field != "value") {
- + ]
1221 : 0 : return false;
1222 : : }
1223 : :
1224 : 2 : const size_t element_size = audio_element_size(message.format());
1225 : 2 : const uint8_t* data = message.data() + index * element_size;
1226 : :
1227 [ - - + - : 2 : switch (message.format()) {
- - ]
1228 : 0 : case AudioFrame::kFormatPcmU8:
1229 : 0 : return store_number(read_unaligned<uint8_t>(data), out);
1230 : 0 : case AudioFrame::kFormatPcmS16:
1231 : 0 : return store_number(read_unaligned<int16_t>(data), out);
1232 : 2 : case AudioFrame::kFormatPcmS24: {
1233 : 2 : const int32_t magnitude = static_cast<int32_t>(read_unaligned<uint8_t>(data)) |
1234 : 2 : (static_cast<int32_t>(read_unaligned<uint8_t>(data + 1)) << 8U) |
1235 : 2 : (static_cast<int32_t>(read_unaligned<uint8_t>(data + 2)) << 16U);
1236 : 2 : return store_number((magnitude ^ 0x800000) - 0x800000, out);
1237 : : }
1238 : :
1239 : 0 : case AudioFrame::kFormatPcmS32:
1240 : 0 : return store_number(read_unaligned<int32_t>(data), out);
1241 : 0 : case AudioFrame::kFormatPcmF32:
1242 : 0 : return store_number(read_unaligned<float>(data), out);
1243 : 0 : default:
1244 : 0 : return false;
1245 : : }
1246 : : }
1247 : :
1248 [ + - - + : 4 : if (type_ != Type::kObjectArray || collection != "data") {
- + ]
1249 : 0 : return false;
1250 : : }
1251 : :
1252 : 4 : const auto* object = get<ObjectArray>()->objects(static_cast<uint32_t>(index));
1253 : :
1254 [ - + ]: 4 : if VUNLIKELY (object == nullptr) {
1255 : 0 : return false;
1256 : : }
1257 : :
1258 [ + + ]: 4 : if (field == "label") {
1259 [ + - ]: 1 : out = std::string(object->label, strnlen(object->label, sizeof(object->label)));
1260 : 1 : return true;
1261 : : }
1262 : :
1263 [ + - + + ]: 3 : if (read_number(field, "yaw", object->yaw, out)) {
1264 : 1 : return true;
1265 : : }
1266 : :
1267 [ + - - + ]: 2 : if (read_number(field, "yaw_rate", object->yaw_rate, out)) {
1268 : 0 : return true;
1269 : : }
1270 : :
1271 [ + - - + ]: 2 : if (read_number(field, "score", object->score, out)) {
1272 : 0 : return true;
1273 : : }
1274 : :
1275 [ + - - + ]: 2 : if (read_number(field, "existence_probability", object->existence_probability, out)) {
1276 : 0 : return true;
1277 : : }
1278 : :
1279 [ + - - + ]: 2 : if (read_number(field, "class_id", object->class_id, out)) {
1280 : 0 : return true;
1281 : : }
1282 : :
1283 [ + - - + ]: 2 : if (read_number(field, "track_id", object->track_id, out)) {
1284 : 0 : return true;
1285 : : }
1286 : :
1287 [ + - - + ]: 2 : if (read_number(field, "age", object->age, out)) {
1288 : 0 : return true;
1289 : : }
1290 : :
1291 [ + - - + ]: 2 : if (read_number(field, "num_observations", object->num_observations, out)) {
1292 : 0 : return true;
1293 : : }
1294 : :
1295 [ + - - + ]: 2 : if (read_number(field, "motion_state", object->motion_state, out)) {
1296 : 0 : return true;
1297 : : }
1298 : :
1299 [ + - - + ]: 2 : if (read_number(field, "source_type", object->source_type, out)) {
1300 : 0 : return true;
1301 : : }
1302 : :
1303 [ + - - + ]: 2 : if (read_number(field, "subtype_id", object->subtype_id, out)) {
1304 : 0 : return true;
1305 : : }
1306 : :
1307 [ + - - + ]: 2 : if (read_number(field, "reserved_buf", object->reserved_buf, out)) {
1308 : 0 : return true;
1309 : : }
1310 : :
1311 [ + + ]: 2 : if (field == "reserved") {
1312 : 1 : return store_number(object->reserved_buf, out);
1313 : : }
1314 : :
1315 : : static constexpr std::pair<std::string_view, uint8_t> kVectorFields[] = {
1316 : : {"position", 3}, {"size", 3}, {"velocity", 3}, {"acceleration", 3}, {"position_covariance", 6}};
1317 : :
1318 [ + + ]: 6 : for (const auto& [name, count] : kVectorFields) {
1319 : 5 : size_t component = 0;
1320 : 5 : std::string_view tail;
1321 : :
1322 [ - + - - : 5 : if (!parse_index(field, name, component, tail) || !tail.empty() || component >= count) {
- - + - ]
1323 : 5 : continue;
1324 : : }
1325 : :
1326 [ # # ]: 0 : if (name == "position") {
1327 : 0 : return store_number(object->position[component], out);
1328 : : }
1329 : :
1330 [ # # ]: 0 : if (name == "size") {
1331 : 0 : return store_number(object->size[component], out);
1332 : : }
1333 : :
1334 [ # # ]: 0 : if (name == "velocity") {
1335 : 0 : return store_number(object->velocity[component], out);
1336 : : }
1337 : :
1338 [ # # ]: 0 : if (name == "acceleration") {
1339 : 0 : return store_number(object->acceleration[component], out);
1340 : : }
1341 : :
1342 : 0 : return store_number(object->position_covariance[component], out);
1343 : : }
1344 : :
1345 : : static constexpr std::pair<std::string_view, std::pair<std::string_view, size_t>> kAliases[] = {
1346 : : {"position_x", {"position", 0}},
1347 : : {"position_y", {"position", 1}},
1348 : : {"position_z", {"position", 2}},
1349 : : {"size_x", {"size", 0}},
1350 : : {"size_y", {"size", 1}},
1351 : : {"size_z", {"size", 2}},
1352 : : {"velocity_x", {"velocity", 0}},
1353 : : {"velocity_y", {"velocity", 1}},
1354 : : {"velocity_z", {"velocity", 2}},
1355 : : {"acceleration_x", {"acceleration", 0}},
1356 : : {"acceleration_y", {"acceleration", 1}},
1357 : : {"acceleration_z", {"acceleration", 2}}};
1358 : :
1359 [ + - ]: 1 : for (const auto& [alias, target] : kAliases) {
1360 [ - + ]: 1 : if (field != alias) {
1361 : 0 : continue;
1362 : : }
1363 : :
1364 [ + - ]: 1 : if (target.first == "position") {
1365 : 1 : return store_number(object->position[target.second], out);
1366 : : }
1367 : :
1368 [ # # ]: 0 : if (target.first == "size") {
1369 : 0 : return store_number(object->size[target.second], out);
1370 : : }
1371 : :
1372 [ # # ]: 0 : if (target.first == "velocity") {
1373 : 0 : return store_number(object->velocity[target.second], out);
1374 : : }
1375 : :
1376 : 0 : return store_number(object->acceleration[target.second], out);
1377 : : }
1378 : :
1379 : 0 : return false;
1380 : : }
1381 : :
1382 : 15 : bool MessageParser::point_value(size_t index, const Field& field, Value& out) const {
1383 : 15 : const auto& message = *get<PointCloud>();
1384 : :
1385 [ + + + - : 15 : if (message.get_extent() != 0 && field.element_index < 3) {
+ + ]
1386 : 5 : return store_number(message.get_value<double>(index, field.byte_offset), out);
1387 : : }
1388 : :
1389 [ - - - - : 10 : switch (field.native_type) {
- - - - -
+ - + - ]
1390 : 0 : case PointCloud::kBoolType:
1391 : 0 : return store_number(message.get_value<bool>(index, field.byte_offset), out);
1392 : 0 : case PointCloud::kInt8Type:
1393 : 0 : return store_number(message.get_value<int8_t>(index, field.byte_offset), out);
1394 : 0 : case PointCloud::kUint8Type:
1395 : 0 : return store_number(message.get_value<uint8_t>(index, field.byte_offset), out);
1396 : 0 : case PointCloud::kInt16Type:
1397 : 0 : return store_number(message.get_value<int16_t>(index, field.byte_offset), out);
1398 : 0 : case PointCloud::kUint16Type:
1399 : 0 : return store_number(message.get_value<uint16_t>(index, field.byte_offset), out);
1400 : 0 : case PointCloud::kInt32Type:
1401 : 0 : return store_number(message.get_value<int32_t>(index, field.byte_offset), out);
1402 : 0 : case PointCloud::kUint32Type:
1403 : 0 : return store_number(message.get_value<uint32_t>(index, field.byte_offset), out);
1404 : 0 : case PointCloud::kInt64Type:
1405 : 0 : return store_number(message.get_value<int64_t>(index, field.byte_offset), out);
1406 : 0 : case PointCloud::kUint64Type:
1407 : 0 : return store_number(message.get_value<uint64_t>(index, field.byte_offset), out);
1408 : 2 : case PointCloud::kFloatType:
1409 : 2 : return store_number(message.get_value<float>(index, field.byte_offset), out);
1410 : 0 : case PointCloud::kDoubleType:
1411 : 0 : return store_number(message.get_value<double>(index, field.byte_offset), out);
1412 : 8 : case PointCloud::kUnknownType:
1413 : 8 : break;
1414 : 0 : default:
1415 : 0 : return false;
1416 : : }
1417 : :
1418 [ + + + + : 8 : switch (field.storage_size) {
+ ]
1419 : 2 : case sizeof(uint8_t):
1420 : 2 : return store_number(message.get_value<uint8_t>(index, field.byte_offset), out);
1421 : 1 : case sizeof(int16_t):
1422 : 1 : return store_number(message.get_value<int16_t>(index, field.byte_offset), out);
1423 : 1 : case sizeof(float):
1424 : 1 : return store_number(message.get_value<float>(index, field.byte_offset), out);
1425 : 1 : case sizeof(double):
1426 : 1 : return store_number(message.get_value<double>(index, field.byte_offset), out);
1427 : 3 : default:
1428 : 3 : return false;
1429 : : }
1430 : : }
1431 : :
1432 : 50 : size_t MessageParser::collection_size(std::string_view collection) const noexcept {
1433 : 50 : collection = normalize_collection(collection);
1434 : :
1435 [ + + + - : 50 : if (type_ == Type::kPointCloud && collection == "data") {
+ + ]
1436 : 19 : return get<PointCloud>()->size();
1437 : : }
1438 : :
1439 [ + + + - : 31 : if (type_ == Type::kObjectArray && collection == "data") {
+ + ]
1440 : 5 : return get<ObjectArray>()->count();
1441 : : }
1442 : :
1443 [ + + + - : 26 : if (type_ == Type::kOccupancyGrid && collection == "data") {
+ + ]
1444 : 3 : const auto& message = *get<OccupancyGrid>();
1445 : :
1446 [ - + ]: 3 : if VUNLIKELY (message.cell_size() == 0) {
1447 : 0 : return 0;
1448 : : }
1449 : :
1450 : 3 : const size_t payload_count = message.size() / message.cell_size();
1451 : 3 : const size_t declared_count = static_cast<size_t>(message.width()) * message.height();
1452 : 3 : return std::min(payload_count, declared_count);
1453 : : }
1454 : :
1455 [ + + ]: 23 : if (type_ == Type::kTensor) {
1456 : 19 : const auto& message = *get<Tensor>();
1457 : :
1458 [ + + - + : 19 : if (collection == "shape" || collection == "strides") {
+ + ]
1459 : 3 : return message.rank();
1460 : : }
1461 : :
1462 [ + - + - : 16 : if (collection == "data" && message.element_size() != 0) {
+ - ]
1463 : 16 : return std::min<size_t>(message.num_elements(), message.size() / message.element_size());
1464 : : }
1465 : : }
1466 : :
1467 [ + - + - : 4 : if (type_ == Type::kAudioFrame && collection == "data") {
+ - ]
1468 : 4 : const auto& message = *get<AudioFrame>();
1469 : 4 : const size_t element_size = audio_element_size(message.format());
1470 : :
1471 [ + - - + : 4 : if VUNLIKELY (element_size == 0 || message.bit_depth() != element_size * 8U) {
- + ]
1472 : 0 : return 0;
1473 : : }
1474 : :
1475 : 4 : return message.size() / element_size;
1476 : : }
1477 : :
1478 : 0 : return 0;
1479 : : }
1480 : :
1481 : 10 : std::vector<MessageParser::Field> MessageParser::fields() const {
1482 : 10 : std::vector<Field> result;
1483 : :
1484 [ - + ]: 10 : if VUNLIKELY (!valid()) {
1485 : 0 : return result;
1486 : : }
1487 : :
1488 [ + - + - ]: 19 : const auto add_text = [&result](std::string_view name) { result.push_back({std::string(name), ValueType::kString}); };
1489 : :
1490 : 75 : const auto add_number = [&result](std::string_view name, ValueType type) {
1491 [ + - + - ]: 75 : result.push_back({std::string(name), type});
1492 : 75 : };
1493 : :
1494 [ + - + - ]: 10 : const auto add_bytes = [&result](std::string_view name) { result.push_back({std::string(name), ValueType::kBytes}); };
1495 : :
1496 : 44 : const auto add_time = [&result](std::string_view name) {
1497 [ + - ]: 22 : Field field{std::string(name), ValueType::kUInt64};
1498 : 22 : field.is_time = true;
1499 [ + - ]: 22 : result.push_back(std::move(field));
1500 : 22 : };
1501 : :
1502 : 2 : const auto add_bool = [&result](std::string_view name) {
1503 [ + - ]: 1 : Field field{std::string(name), ValueType::kUInt64};
1504 : 1 : field.is_bool = true;
1505 [ + - ]: 1 : result.push_back(std::move(field));
1506 : 1 : };
1507 : :
1508 : 22 : const auto add_enum = [&result](std::string_view name, EnumKind kind) {
1509 [ + - ]: 11 : Field field{std::string(name), ValueType::kUInt64};
1510 : 11 : field.enum_kind = kind;
1511 [ + - ]: 11 : result.push_back(std::move(field));
1512 : 11 : };
1513 : :
1514 : 64 : const auto add_reserved = [&result](std::string_view name) {
1515 [ + - ]: 32 : Field field{std::string(name), ValueType::kUInt64};
1516 : 32 : field.is_reserved = true;
1517 [ + - ]: 32 : result.push_back(std::move(field));
1518 : 32 : };
1519 : :
1520 [ + + ]: 10 : if (type_ != Type::kProxyData) {
1521 [ + - ]: 9 : add_text("header.frame_id");
1522 [ + - ]: 9 : add_number("header.seq", ValueType::kUInt64);
1523 [ + - ]: 9 : add_reserved("header.reserved");
1524 [ + - ]: 9 : add_time("header.time_meas");
1525 [ + - ]: 9 : add_time("header.time_pub");
1526 : : }
1527 : :
1528 [ + + + + : 10 : switch (type_) {
+ + + +
- ]
1529 : 1 : case Type::kRawData:
1530 [ + - ]: 1 : add_number("size", ValueType::kUInt64);
1531 [ + - ]: 1 : add_reserved("reserved");
1532 [ + - ]: 1 : add_bytes("data");
1533 : 1 : break;
1534 : :
1535 : 3 : case Type::kCameraFrame:
1536 [ + - ]: 3 : add_number("channel", ValueType::kUInt64);
1537 [ + - ]: 3 : add_number("height", ValueType::kUInt64);
1538 [ + - ]: 3 : add_number("width", ValueType::kUInt64);
1539 [ + - ]: 3 : add_number("freq", ValueType::kUInt64);
1540 [ + - ]: 3 : add_enum("format", EnumKind::kEnumCameraFormat);
1541 [ + - ]: 3 : add_enum("stream", EnumKind::kEnumCameraStream);
1542 [ + - ]: 3 : add_number("size", ValueType::kUInt64);
1543 [ + - ]: 3 : add_reserved("reserved");
1544 [ + - ]: 3 : add_bytes("data");
1545 : 3 : break;
1546 : :
1547 : 1 : case Type::kPointCloud:
1548 [ + - ]: 1 : add_number("size", ValueType::kUInt64);
1549 [ + - ]: 1 : add_number("pack_size", ValueType::kUInt64);
1550 [ + - ]: 1 : add_number("extent", ValueType::kUInt64);
1551 [ + - ]: 1 : add_number("downsample", ValueType::kUInt64);
1552 [ + - ]: 1 : add_bool("vertical");
1553 [ + - ]: 1 : add_reserved("reserved_size");
1554 [ + - ]: 1 : add_reserved("reserved");
1555 [ + - ]: 1 : add_reserved("reserved2");
1556 [ + - ]: 1 : add_reserved("reserved3");
1557 [ + - ]: 1 : add_bytes("data");
1558 : 1 : break;
1559 : :
1560 : 1 : case Type::kProxyData:
1561 [ + - ]: 1 : add_number("control_id", ValueType::kUInt64);
1562 [ + - ]: 1 : add_number("mode", ValueType::kUInt64);
1563 [ + - ]: 1 : add_number("timestamp", ValueType::kInt64);
1564 [ + - ]: 1 : add_number("seq", ValueType::kInt64);
1565 [ + - ]: 1 : add_number("schema", ValueType::kUInt64);
1566 [ + - ]: 1 : add_text("url");
1567 [ + - ]: 1 : add_text("ser");
1568 [ + - ]: 1 : add_text("hostname");
1569 [ + - ]: 1 : add_number("size", ValueType::kUInt64);
1570 [ + - ]: 1 : add_reserved("reserved");
1571 [ + - ]: 1 : add_reserved("reserved2");
1572 [ + - ]: 1 : add_bytes("raw");
1573 : 1 : break;
1574 : :
1575 : 1 : case Type::kOccupancyGrid:
1576 [ + - ]: 1 : add_text("map_id");
1577 [ + - ]: 1 : add_number("width", ValueType::kUInt64);
1578 [ + - ]: 1 : add_number("height", ValueType::kUInt64);
1579 [ + - ]: 1 : add_number("channel", ValueType::kUInt64);
1580 [ + - ]: 1 : add_number("freq", ValueType::kUInt64);
1581 [ + - ]: 1 : add_number("valid_cell_count", ValueType::kUInt64);
1582 [ + - ]: 1 : add_number("default_value", ValueType::kInt64);
1583 [ + - ]: 1 : add_number("resolution", ValueType::kDouble);
1584 [ + - ]: 1 : add_number("origin_x", ValueType::kDouble);
1585 [ + - ]: 1 : add_number("origin_y", ValueType::kDouble);
1586 [ + - ]: 1 : add_number("origin_z", ValueType::kDouble);
1587 [ + - ]: 1 : add_number("origin_yaw", ValueType::kDouble);
1588 [ + - ]: 1 : add_number("value_min", ValueType::kDouble);
1589 [ + - ]: 1 : add_number("value_max", ValueType::kDouble);
1590 [ + - ]: 1 : add_number("occupied_threshold", ValueType::kDouble);
1591 [ + - ]: 1 : add_number("free_threshold", ValueType::kDouble);
1592 [ + - ]: 1 : add_enum("cell_type", EnumKind::kEnumGridCellType);
1593 [ + - ]: 1 : add_number("cell_size", ValueType::kUInt64);
1594 [ + - ]: 1 : add_time("update_time_ns");
1595 [ + - ]: 1 : add_number("size", ValueType::kUInt64);
1596 [ + - ]: 1 : add_reserved("reserved");
1597 [ + - ]: 1 : add_reserved("reserved2");
1598 [ + - ]: 1 : add_reserved("reserved3");
1599 [ + - ]: 1 : add_bytes("data");
1600 : 1 : break;
1601 : :
1602 : 1 : case Type::kTensor:
1603 [ + - ]: 1 : add_text("name");
1604 [ + - ]: 1 : add_text("model_id");
1605 [ + - ]: 1 : add_text("layout");
1606 [ + - ]: 1 : add_enum("dtype", EnumKind::kEnumTensorDataType);
1607 [ + - ]: 1 : add_enum("device", EnumKind::kEnumTensorDevice);
1608 [ + - ]: 1 : add_number("rank", ValueType::kUInt64);
1609 [ + - ]: 1 : add_number("num_elements", ValueType::kUInt64);
1610 [ + - ]: 1 : add_number("element_size", ValueType::kUInt64);
1611 [ + - ]: 1 : add_number("batch_size", ValueType::kUInt64);
1612 [ + - ]: 1 : add_number("channel", ValueType::kUInt64);
1613 [ + - ]: 1 : add_number("freq", ValueType::kUInt64);
1614 [ + - ]: 1 : add_number("quant_zero_point", ValueType::kInt64);
1615 [ + - ]: 1 : add_number("quant_scale", ValueType::kDouble);
1616 [ + - ]: 1 : add_time("update_time_ns");
1617 [ + - ]: 1 : add_number("size", ValueType::kUInt64);
1618 [ + - ]: 1 : add_reserved("reserved");
1619 [ + - ]: 1 : add_reserved("reserved2");
1620 [ + - ]: 1 : add_reserved("reserved3");
1621 [ + - ]: 1 : add_bytes("data");
1622 : 1 : break;
1623 : :
1624 : 1 : case Type::kObjectArray:
1625 [ + - ]: 1 : add_text("source_id");
1626 [ + - ]: 1 : add_number("channel", ValueType::kUInt64);
1627 [ + - ]: 1 : add_number("freq", ValueType::kUInt64);
1628 [ + - ]: 1 : add_number("count", ValueType::kUInt64);
1629 [ + - ]: 1 : add_number("pack_size", ValueType::kUInt64);
1630 [ + - ]: 1 : add_time("update_time_ns");
1631 [ + - ]: 1 : add_number("size", ValueType::kUInt64);
1632 [ + - ]: 1 : add_reserved("reserved");
1633 [ + - ]: 1 : add_reserved("reserved2");
1634 [ + - ]: 1 : add_reserved("reserved3");
1635 [ + - ]: 1 : add_reserved("reserved4");
1636 [ + - ]: 1 : add_reserved("reserved5");
1637 [ + - ]: 1 : add_bytes("data");
1638 : 1 : break;
1639 : :
1640 : 1 : case Type::kAudioFrame:
1641 [ + - ]: 1 : add_text("codec");
1642 [ + - ]: 1 : add_text("language");
1643 [ + - ]: 1 : add_number("channel", ValueType::kUInt64);
1644 [ + - ]: 1 : add_number("freq", ValueType::kUInt64);
1645 [ + - ]: 1 : add_number("sample_rate", ValueType::kUInt64);
1646 [ + - ]: 1 : add_number("num_samples", ValueType::kUInt64);
1647 [ + - ]: 1 : add_number("num_channels", ValueType::kUInt64);
1648 [ + - ]: 1 : add_number("bit_depth", ValueType::kUInt64);
1649 [ + - ]: 1 : add_number("bitrate", ValueType::kUInt64);
1650 [ + - ]: 1 : add_enum("format", EnumKind::kEnumAudioFormat);
1651 [ + - ]: 1 : add_enum("layout", EnumKind::kEnumAudioLayout);
1652 [ + - ]: 1 : add_number("duration_ns", ValueType::kUInt64);
1653 [ + - ]: 1 : add_time("update_time_ns");
1654 [ + - ]: 1 : add_number("size", ValueType::kUInt64);
1655 [ + - ]: 1 : add_reserved("reserved");
1656 [ + - ]: 1 : add_reserved("reserved2");
1657 [ + - ]: 1 : add_bytes("data");
1658 : 1 : break;
1659 : 0 : default:
1660 : 0 : break;
1661 : : }
1662 : :
1663 : 10 : return result;
1664 : 0 : }
1665 : :
1666 : 5 : std::vector<MessageParser::Field> MessageParser::element_fields(std::string_view collection) const {
1667 : 5 : collection = normalize_collection(collection);
1668 : :
1669 [ + + + - : 5 : if (type_ == Type::kPointCloud && collection == "data") {
+ + ]
1670 : 3 : return point_fields_;
1671 : : }
1672 : :
1673 [ - + - - : 2 : if (type_ == Type::kTensor && collection == "data") {
- + ]
1674 : 0 : const auto& message = *get<Tensor>();
1675 [ # # # # : 0 : return {{"value", tensor_value_type(message.dtype()), message.dtype(), message.element_size()}};
# # # # ]
1676 : : }
1677 : :
1678 [ - + - - : 2 : if (type_ == Type::kTensor && (collection == "shape" || collection == "strides")) {
- - - + ]
1679 [ # # # # : 0 : return {{"value", ValueType::kUInt64}};
# # # # ]
1680 : : }
1681 : :
1682 [ - + - - : 2 : if (type_ == Type::kOccupancyGrid && collection == "data") {
- + ]
1683 : 0 : const auto& message = *get<OccupancyGrid>();
1684 : :
1685 [ # # # # ]: 0 : switch (message.cell_type()) {
1686 : 0 : case OccupancyGrid::kCellInt8:
1687 [ # # # # : 0 : return {{"value", ValueType::kInt64, message.cell_type(), message.cell_size()}};
# # # # ]
1688 : 0 : case OccupancyGrid::kCellUint8:
1689 : : case OccupancyGrid::kCellUint16:
1690 [ # # # # : 0 : return {{"value", ValueType::kUInt64, message.cell_type(), message.cell_size()}};
# # # # ]
1691 : 0 : case OccupancyGrid::kCellFloat32:
1692 [ # # # # : 0 : return {{"value", ValueType::kDouble, message.cell_type(), message.cell_size()}};
# # # # ]
1693 : 0 : default:
1694 : 0 : return {};
1695 : : }
1696 : : }
1697 : :
1698 [ + + + - : 2 : if (type_ == Type::kAudioFrame && collection == "data") {
+ + ]
1699 : 1 : const auto& message = *get<AudioFrame>();
1700 : :
1701 [ - - + - : 1 : switch (message.format()) {
- - ]
1702 : 0 : case AudioFrame::kFormatPcmU8:
1703 [ # # # # : 0 : return {{"value", ValueType::kUInt64, message.format(), static_cast<uint16_t>(sizeof(uint8_t))}};
# # # # ]
1704 : 0 : case AudioFrame::kFormatPcmS16:
1705 [ # # # # : 0 : return {{"value", ValueType::kInt64, message.format(), static_cast<uint16_t>(sizeof(int16_t))}};
# # # # ]
1706 : 1 : case AudioFrame::kFormatPcmS24:
1707 [ + - + - : 2 : return {{"value", ValueType::kInt64, message.format(), 3}};
+ + - - ]
1708 : 0 : case AudioFrame::kFormatPcmS32:
1709 [ # # # # : 0 : return {{"value", ValueType::kInt64, message.format(), static_cast<uint16_t>(sizeof(int32_t))}};
# # # # ]
1710 : 0 : case AudioFrame::kFormatPcmF32:
1711 [ # # # # : 0 : return {{"value", ValueType::kDouble, message.format(), static_cast<uint16_t>(sizeof(float))}};
# # # # ]
1712 : 0 : default:
1713 : 0 : return {};
1714 : : }
1715 : : }
1716 : :
1717 [ - + - - : 1 : if (type_ == Type::kObjectArray && collection == "data") {
- + ]
1718 : : return {{"label", ValueType::kString},
1719 : : {"position[0]", ValueType::kDouble},
1720 : : {"position[1]", ValueType::kDouble},
1721 : : {"position[2]", ValueType::kDouble},
1722 : : {"yaw", ValueType::kDouble},
1723 : : {"size[0]", ValueType::kDouble},
1724 : : {"size[1]", ValueType::kDouble},
1725 : : {"size[2]", ValueType::kDouble},
1726 : : {"yaw_rate", ValueType::kDouble},
1727 : : {"velocity[0]", ValueType::kDouble},
1728 : : {"velocity[1]", ValueType::kDouble},
1729 : : {"velocity[2]", ValueType::kDouble},
1730 : : {"score", ValueType::kDouble},
1731 : : {"acceleration[0]", ValueType::kDouble},
1732 : : {"acceleration[1]", ValueType::kDouble},
1733 : : {"acceleration[2]", ValueType::kDouble},
1734 : : {"existence_probability", ValueType::kDouble},
1735 : : {"position_covariance[0]", ValueType::kDouble},
1736 : : {"position_covariance[1]", ValueType::kDouble},
1737 : : {"position_covariance[2]", ValueType::kDouble},
1738 : : {"position_covariance[3]", ValueType::kDouble},
1739 : : {"position_covariance[4]", ValueType::kDouble},
1740 : : {"position_covariance[5]", ValueType::kDouble},
1741 : : {"class_id", ValueType::kUInt64},
1742 : : {"track_id", ValueType::kUInt64},
1743 : : {"age", ValueType::kUInt64},
1744 : : {"num_observations", ValueType::kUInt64},
1745 : : {"motion_state", ValueType::kUInt64},
1746 : : {"source_type", ValueType::kUInt64},
1747 : : {"subtype_id", ValueType::kUInt64},
1748 [ # # # # : 0 : {"reserved", ValueType::kUInt64}};
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # ]
1749 : : }
1750 : :
1751 : 1 : return {};
1752 : : }
1753 : :
1754 : 6 : static std::string format_integer(const MessageParser::Value& value, bool hex) {
1755 [ - + ]: 6 : if (const auto* number = std::get_if<int64_t>(&value)) {
1756 [ # # ]: 0 : return hex ? Helpers::format_hex_number(*number) : std::to_string(*number);
1757 : : }
1758 : :
1759 [ + - ]: 6 : if (const auto* number = std::get_if<uint64_t>(&value)) {
1760 [ - + ]: 6 : return hex ? Helpers::format_hex_number(*number) : std::to_string(*number);
1761 : : }
1762 : :
1763 [ # # ]: 0 : if (const auto* number = std::get_if<double>(&value)) {
1764 : 0 : return std::to_string(*number);
1765 : : }
1766 : :
1767 : 0 : return {};
1768 : : }
1769 : :
1770 : 2 : static std::string_view enum_label(MessageParser::EnumKind kind, uint64_t value) {
1771 [ + + - - : 2 : switch (kind) {
- - - - ]
1772 : 1 : case MessageParser::EnumKind::kEnumCameraFormat:
1773 : 1 : return NameDetector::get_enum(static_cast<CameraFrame::Format>(value));
1774 : 1 : case MessageParser::EnumKind::kEnumCameraStream:
1775 : 1 : return NameDetector::get_enum(static_cast<CameraFrame::Stream>(value));
1776 : 0 : case MessageParser::EnumKind::kEnumGridCellType:
1777 : 0 : return NameDetector::get_enum(static_cast<OccupancyGrid::CellType>(value));
1778 : 0 : case MessageParser::EnumKind::kEnumTensorDataType:
1779 : 0 : return NameDetector::get_enum(static_cast<Tensor::DataType>(value));
1780 : 0 : case MessageParser::EnumKind::kEnumTensorDevice:
1781 : 0 : return NameDetector::get_enum(static_cast<Tensor::Device>(value));
1782 : 0 : case MessageParser::EnumKind::kEnumAudioFormat:
1783 : 0 : return NameDetector::get_enum(static_cast<AudioFrame::Format>(value));
1784 : 0 : case MessageParser::EnumKind::kEnumAudioLayout:
1785 : 0 : return NameDetector::get_enum(static_cast<AudioFrame::Layout>(value));
1786 : 0 : default:
1787 : 0 : return {};
1788 : : }
1789 : : }
1790 : :
1791 : 11 : static std::string format_scalar(const MessageParser& parser, const MessageParser::Field& field,
1792 : : const MessageFormatOptions& options) {
1793 : 11 : MessageParser::Value value;
1794 : :
1795 [ + - - + ]: 11 : if VUNLIKELY (!parser.value(field.name, value)) {
1796 : 0 : return {};
1797 : : }
1798 : :
1799 [ + + ]: 11 : if (const auto* text = std::get_if<std::string>(&value)) {
1800 [ + - ]: 1 : return *text;
1801 : : }
1802 : :
1803 [ - + ]: 10 : if (field.is_bool) {
1804 : 0 : const auto* number = std::get_if<uint64_t>(&value);
1805 [ # # # # : 0 : return number != nullptr && *number != 0 ? "true" : "false";
# # ]
1806 : : }
1807 : :
1808 [ + + ]: 10 : if (field.enum_kind != MessageParser::EnumKind::kEnumNone) {
1809 : 2 : const auto* number = std::get_if<uint64_t>(&value);
1810 [ + - ]: 2 : const uint64_t raw = number != nullptr ? *number : 0;
1811 [ + - + - : 4 : return options.enum_name ? std::string(enum_label(field.enum_kind, raw)) : std::to_string(raw);
- - + - -
- ]
1812 : : }
1813 : :
1814 [ + + ]: 8 : if (field.is_time) {
1815 : 2 : const auto* number = std::get_if<uint64_t>(&value);
1816 [ + - ]: 2 : const uint64_t raw = number != nullptr ? *number : 0;
1817 : :
1818 [ - + ]: 2 : if (options.date) {
1819 : 0 : return Helpers::format_date(static_cast<int64_t>(raw));
1820 : : }
1821 : :
1822 [ - + + - ]: 2 : return options.hex ? Helpers::format_hex_number(raw) : std::to_string(raw);
1823 : : }
1824 : :
1825 [ + - ]: 6 : return format_integer(value, options.hex);
1826 : 11 : }
1827 : :
1828 : 0 : static std::string format_pointcloud_protocol(const MessageParser& parser) {
1829 : : static constexpr std::array<std::string_view, 12> kTypeNames = {
1830 : : "unknown", "bool", "int8", "uint8", "int16", "uint16", "int32", "uint32", "int64", "uint64", "float", "double"};
1831 : :
1832 [ # # ]: 0 : const auto fields = parser.element_fields("data");
1833 : :
1834 : 0 : uint64_t extent = 0;
1835 : 0 : MessageParser::Value extent_value;
1836 : :
1837 [ # # # # ]: 0 : if (parser.value("extent", extent_value)) {
1838 [ # # ]: 0 : if (const auto* number = std::get_if<uint64_t>(&extent_value)) {
1839 : 0 : extent = *number;
1840 : : }
1841 : : }
1842 : :
1843 : 0 : std::string size_list;
1844 : 0 : std::string name_list;
1845 : 0 : std::string type_list;
1846 : :
1847 [ # # ]: 0 : for (size_t i = 0; i < fields.size(); ++i) {
1848 [ # # ]: 0 : if (i != 0) {
1849 [ # # ]: 0 : size_list += ",";
1850 [ # # ]: 0 : name_list += ",";
1851 [ # # ]: 0 : type_list += ",";
1852 : : }
1853 : :
1854 : 0 : const auto& field = fields[i];
1855 : 0 : auto native_type = field.native_type;
1856 : :
1857 [ # # # # : 0 : if (extent != 0 && i < 3 && native_type == PointCloud::kInt16Type && field.storage_size == sizeof(int16_t)) {
# # # # ]
1858 : 0 : native_type = PointCloud::kFloatType;
1859 : : }
1860 : :
1861 [ # # # # ]: 0 : size_list += std::to_string(field.storage_size);
1862 [ # # ]: 0 : name_list += field.name;
1863 [ # # # # : 0 : type_list += native_type < kTypeNames.size() ? std::string(kTypeNames[native_type]) : "unknown";
# # # # #
# # # # #
# # ]
1864 : : }
1865 : :
1866 [ # # ]: 0 : std::string result = "protocol {\n";
1867 [ # # # # : 0 : result += " size_list: " + size_list + "\n";
# # ]
1868 [ # # # # : 0 : result += " name_list: " + name_list + "\n";
# # ]
1869 [ # # # # : 0 : result += " type_list: " + type_list + "\n";
# # ]
1870 [ # # ]: 0 : result += "}\n";
1871 : :
1872 : 0 : return result;
1873 : 0 : }
1874 : :
1875 : 0 : static std::string format_tensor_shape(const MessageParser& parser) {
1876 [ # # ]: 0 : std::string shape = "[";
1877 : 0 : const size_t count = parser.collection_size("shape");
1878 : :
1879 [ # # ]: 0 : for (size_t d = 0; d < count; ++d) {
1880 [ # # ]: 0 : if (d != 0) {
1881 [ # # ]: 0 : shape += ", ";
1882 : : }
1883 : :
1884 : 0 : MessageParser::Value value;
1885 : :
1886 [ # # # # ]: 0 : if (parser.value("shape", d, std::string_view{}, value)) {
1887 [ # # ]: 0 : if (const auto* number = std::get_if<uint64_t>(&value)) {
1888 [ # # # # ]: 0 : shape += std::to_string(*number);
1889 : : }
1890 : : }
1891 : 0 : }
1892 : :
1893 [ # # ]: 0 : shape += "]";
1894 : :
1895 [ # # # # ]: 0 : return "shape: " + shape + "\n";
1896 : 0 : }
1897 : :
1898 : 0 : static std::string format_pointcloud_points(const MessageParser& parser, const MessageFormatOptions& options,
1899 : : bool* truncated) {
1900 [ # # ]: 0 : const auto fields = parser.element_fields("data");
1901 : 0 : const size_t total = parser.collection_size("data");
1902 : 0 : const size_t count = std::min(total, options.max_elements);
1903 : :
1904 [ # # # # ]: 0 : if (truncated != nullptr && total > options.max_elements) {
1905 : 0 : *truncated = true;
1906 : : }
1907 : :
1908 : 0 : std::string result;
1909 : :
1910 [ # # ]: 0 : for (size_t i = 0; i < count; ++i) {
1911 [ # # # # : 0 : result += "data[" + std::to_string(i) + "] {\n";
# # # # ]
1912 : :
1913 [ # # ]: 0 : for (const auto& field : fields) {
1914 : 0 : MessageParser::Value value;
1915 : 0 : std::string text;
1916 : :
1917 [ # # # # ]: 0 : if (parser.value("data", i, field, value)) {
1918 [ # # ]: 0 : if (field.is_bool) {
1919 : 0 : const auto* number = std::get_if<uint64_t>(&value);
1920 [ # # # # : 0 : text = number != nullptr && *number != 0 ? "true" : "false";
# # ]
1921 : : } else {
1922 [ # # ]: 0 : text = format_integer(value, options.hex);
1923 : : }
1924 : : }
1925 : :
1926 [ # # # # : 0 : result += " " + field.name + ": " + text + "\n";
# # # # #
# ]
1927 : 0 : }
1928 : :
1929 [ # # ]: 0 : result += "}\n";
1930 : : }
1931 : :
1932 : 0 : return result;
1933 : 0 : }
1934 : :
1935 : 1 : std::string format_message(const MessageParser& parser, const MessageFormatOptions& options, bool* truncated) {
1936 [ - + ]: 1 : if (truncated != nullptr) {
1937 : 0 : *truncated = false;
1938 : : }
1939 : :
1940 : 1 : std::string result;
1941 : :
1942 [ - + ]: 1 : if VUNLIKELY (!parser.valid()) {
1943 : 0 : return result;
1944 : : }
1945 : :
1946 [ + - ]: 1 : const auto fields = parser.fields();
1947 : 1 : const bool is_point_cloud = parser.type() == MessageParser::Type::kPointCloud;
1948 : 1 : const bool is_tensor = parser.type() == MessageParser::Type::kTensor;
1949 : :
1950 : 1 : std::string header;
1951 : :
1952 : 1 : size_t index = 0;
1953 : :
1954 [ + - ]: 6 : for (; index < fields.size(); ++index) {
1955 : 6 : const auto& field = fields[index];
1956 : :
1957 [ + + ]: 6 : if (!Helpers::has_startwith(field.name, "header.")) {
1958 : 1 : break;
1959 : : }
1960 : :
1961 [ + + ]: 5 : if (field.is_reserved) {
1962 : 1 : continue;
1963 : : }
1964 : :
1965 [ + - + - : 8 : header += " " + field.name.substr(std::string_view("header.").size()) + ": " +
+ - + - ]
1966 [ + - + - : 12 : format_scalar(parser, field, options) + "\n";
+ - ]
1967 : : }
1968 : :
1969 [ + - ]: 1 : if (!header.empty()) {
1970 [ + - ]: 1 : result += "header {\n";
1971 [ + - ]: 1 : result += header;
1972 [ + - ]: 1 : result += "}\n";
1973 : : }
1974 : :
1975 [ - + ]: 1 : if (is_point_cloud) {
1976 [ # # # # ]: 0 : result += format_pointcloud_protocol(parser);
1977 : : }
1978 : :
1979 [ + + ]: 10 : for (; index < fields.size(); ++index) {
1980 : 9 : const auto& field = fields[index];
1981 : :
1982 [ + + ]: 9 : if (field.is_reserved) {
1983 : 1 : continue;
1984 : : }
1985 : :
1986 [ + + ]: 8 : if (field.type == MessageParser::ValueType::kBytes) {
1987 [ - + ]: 1 : if (is_tensor) {
1988 [ # # # # ]: 0 : result += format_tensor_shape(parser);
1989 : : }
1990 : :
1991 [ - + ]: 1 : if (is_point_cloud) {
1992 [ # # ]: 0 : if (options.expand_arrays) {
1993 [ # # # # ]: 0 : result += format_pointcloud_points(parser, options, truncated);
1994 : : }
1995 : : } else {
1996 [ + - + - ]: 1 : result += field.name + ": {...}\n";
1997 : : }
1998 : :
1999 : 1 : continue;
2000 : : }
2001 : :
2002 [ + - + - : 7 : result += field.name + ": " + format_scalar(parser, field, options) + "\n";
+ - + - +
- ]
2003 : : }
2004 : :
2005 : 1 : return result;
2006 : 1 : }
2007 : :
2008 : : } // namespace zerocopy
2009 : :
2010 : : } // namespace vlink
|
