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 "./zerocopy/occupancy_grid.h"
25 : :
26 : : #include <cstdint>
27 : :
28 : : namespace vlink {
29 : :
30 : : namespace zerocopy {
31 : :
32 : : // OccupancyGrid
33 : 58 : OccupancyGrid::OccupancyGrid() noexcept {
34 : : #if defined(__arm__) || defined(__x86__) || defined(__i386__)
35 : : #ifndef __ANDROID__
36 : : #warning "[OccupancyGrid] No support for 32-bit architecture."
37 : : #endif
38 : : #else
39 : : static_assert(sizeof(OccupancyGrid) == 152, "Sizeof must be 152 bytes.");
40 : : #endif
41 : 58 : }
42 : :
43 : 60 : OccupancyGrid::~OccupancyGrid() noexcept {
44 [ + + + - : 60 : if (is_owner_ && data_ && size_ != 0) {
+ - ]
45 : 32 : Bytes::bytes_free(data_, size_);
46 : : }
47 : 60 : }
48 : :
49 : 1 : OccupancyGrid::OccupancyGrid(const OccupancyGrid& target) noexcept { deep_copy(target); }
50 : :
51 : 1 : OccupancyGrid::OccupancyGrid(OccupancyGrid&& target) noexcept { move_copy(target); }
52 : :
53 : 1 : OccupancyGrid& OccupancyGrid::operator=(const OccupancyGrid& target) noexcept {
54 [ - + ]: 1 : if VUNLIKELY (this == &target) {
55 : 0 : return *this;
56 : : }
57 : :
58 : 1 : deep_copy(target);
59 : :
60 : 1 : return *this;
61 : : }
62 : :
63 : 1 : OccupancyGrid& OccupancyGrid::operator=(OccupancyGrid&& target) noexcept {
64 [ - + ]: 1 : if VUNLIKELY (this == &target) {
65 : 0 : return *this;
66 : : }
67 : :
68 : 1 : move_copy(target);
69 : :
70 : 1 : return *this;
71 : : }
72 : :
73 : 9 : bool OccupancyGrid::operator<<(const Bytes& bytes) noexcept {
74 : : static constexpr size_t kMagicNumberBeginSize = sizeof(kMagicNumberBegin);
75 : : static constexpr size_t kVersionSize = sizeof(kWireVersion);
76 : : // static constexpr size_t kMagicNumberEndSize = sizeof(kMagicNumberEnd);
77 : :
78 [ + + ]: 9 : if VUNLIKELY (bytes.empty()) {
79 : 1 : return false;
80 : : }
81 : :
82 [ + + ]: 8 : if VUNLIKELY (!check_valid(bytes)) {
83 : 2 : return false;
84 : : }
85 : :
86 : 6 : uint32_t wire_version = 0;
87 : 6 : std::memcpy(&wire_version, bytes.data() + kMagicNumberBeginSize, kVersionSize);
88 : :
89 [ - + ]: 6 : if VUNLIKELY (version_major(wire_version) != version_major(kWireVersion)) {
90 : 0 : return false;
91 : : }
92 : :
93 [ + + + - : 6 : if (is_owner_ && data_ && size_ != 0) {
+ - ]
94 : 1 : Bytes::bytes_free(data_, size_);
95 : : }
96 : :
97 : : #if defined(__GNUC__) && !defined(__clang__)
98 : : #pragma GCC diagnostic push
99 : : #pragma GCC diagnostic ignored "-Wclass-memaccess"
100 : : #if __GNUC__ >= 11
101 : : #pragma GCC diagnostic ignored "-Wstringop-overread"
102 : : #endif
103 : : #endif
104 : :
105 : 6 : auto* target_ptr = reinterpret_cast<uint8_t*>(this);
106 : :
107 : 6 : std::memcpy(target_ptr, bytes.data() + kMagicNumberBeginSize + kVersionSize, sizeof(OccupancyGrid));
108 : :
109 : : #if defined(__GNUC__) && !defined(__clang__)
110 : : #pragma GCC diagnostic pop
111 : : #endif
112 : :
113 : 6 : data_ = const_cast<uint8_t*>(bytes.data() + kMagicNumberBeginSize + kVersionSize + sizeof(OccupancyGrid));
114 : 6 : is_owner_ = false;
115 : :
116 [ + + ]: 6 : if VUNLIKELY (bytes.size() != get_serialized_size()) {
117 : 1 : clear();
118 : 1 : return false;
119 : : }
120 : :
121 : 5 : return true;
122 : : }
123 : :
124 : 9 : bool OccupancyGrid::operator>>(Bytes& bytes) const noexcept {
125 : : static constexpr size_t kMagicNumberBeginSize = sizeof(kMagicNumberBegin);
126 : : static constexpr size_t kVersionSize = sizeof(kWireVersion);
127 : : static constexpr size_t kMagicNumberEndSize = sizeof(kMagicNumberEnd);
128 : :
129 [ + + + + : 9 : if (bytes.empty() || bytes.size() != get_serialized_size()) {
+ + ]
130 : 8 : bytes = Bytes::create(get_serialized_size());
131 : :
132 [ - + ]: 8 : if VUNLIKELY (bytes.empty()) {
133 : 0 : return false;
134 : : }
135 : : }
136 : :
137 : 9 : std::memcpy(bytes.data(), &kMagicNumberBegin, kMagicNumberBeginSize);
138 : :
139 : 9 : std::memcpy(bytes.data() + kMagicNumberBeginSize, &kWireVersion, kVersionSize);
140 : :
141 : : // NOLINTNEXTLINE(bugprone-undefined-memory-manipulation)
142 : 9 : std::memcpy(bytes.data() + kMagicNumberBeginSize + kVersionSize, this, sizeof(OccupancyGrid));
143 : :
144 : 9 : const auto data_offset = reinterpret_cast<const uint8_t*>(&data_) - reinterpret_cast<const uint8_t*>(this);
145 : 9 : const size_t data_pointer_size = sizeof(data_);
146 : 9 : std::memset(bytes.data() + kMagicNumberBeginSize + kVersionSize + data_offset, 0, data_pointer_size);
147 : :
148 [ + + + - : 9 : if VLIKELY (data_ != nullptr && size_ != 0) {
+ + ]
149 : 8 : std::memcpy(bytes.data() + kMagicNumberBeginSize + kVersionSize + sizeof(OccupancyGrid), data_, size_);
150 : : }
151 : :
152 : 9 : std::memcpy(bytes.data() + kMagicNumberBeginSize + kVersionSize + sizeof(OccupancyGrid) + size_, &kMagicNumberEnd,
153 : : kMagicNumberEndSize);
154 : :
155 : 9 : return true;
156 : : }
157 : :
158 : 15 : bool OccupancyGrid::check_valid(const Bytes& bytes) noexcept {
159 : : static constexpr size_t kMagicNumberBeginSize = sizeof(kMagicNumberBegin);
160 : : static constexpr size_t kVersionSize = sizeof(kWireVersion);
161 : : static constexpr size_t kMagicNumberEndSize = sizeof(kMagicNumberEnd);
162 : :
163 [ + + ]: 15 : if VUNLIKELY (bytes.size() < kMagicNumberBeginSize + kVersionSize + sizeof(OccupancyGrid) + kMagicNumberEndSize) {
164 : 2 : return false;
165 : : }
166 : :
167 : 13 : uint32_t check_magic = 0;
168 : :
169 : 13 : std::memcpy(&check_magic, bytes.begin(), kMagicNumberBeginSize);
170 : :
171 [ + + ]: 13 : if VUNLIKELY (check_magic != kMagicNumberBegin) {
172 : 1 : return false;
173 : : }
174 : :
175 : 12 : uint32_t wire_version = 0;
176 : 12 : std::memcpy(&wire_version, bytes.data() + kMagicNumberBeginSize, kVersionSize);
177 : :
178 [ + + ]: 12 : if VUNLIKELY (version_major(wire_version) != version_major(kWireVersion)) {
179 : 2 : return false;
180 : : }
181 : :
182 : 10 : std::memcpy(&check_magic, bytes.end() - kMagicNumberEndSize, kMagicNumberEndSize);
183 : :
184 [ + + ]: 10 : if VUNLIKELY (check_magic != kMagicNumberEnd) {
185 : 1 : return false;
186 : : }
187 : :
188 : 9 : return true;
189 : : }
190 : :
191 : 20 : size_t OccupancyGrid::get_serialized_size() const noexcept {
192 : : static constexpr size_t kMagicNumberBeginSize = sizeof(kMagicNumberBegin);
193 : : static constexpr size_t kVersionSize = sizeof(kWireVersion);
194 : : static constexpr size_t kMagicNumberEndSize = sizeof(kMagicNumberEnd);
195 : :
196 : 20 : return kMagicNumberBeginSize + kVersionSize + sizeof(OccupancyGrid) + size_ + kMagicNumberEndSize;
197 : : }
198 : :
199 [ + + + - ]: 21 : bool OccupancyGrid::is_valid() const noexcept { return data_ != nullptr && size_ != 0; }
200 : :
201 : 10 : bool OccupancyGrid::shallow_copy(const OccupancyGrid& target) noexcept {
202 [ + + ]: 10 : if VUNLIKELY (this == &target) {
203 : 2 : return false;
204 : : }
205 : :
206 [ + + + - : 8 : if (is_owner_ && data_ && size_ != 0) {
+ - ]
207 : 1 : const auto current = reinterpret_cast<uintptr_t>(data_);
208 : 1 : const auto source = reinterpret_cast<uintptr_t>(target.data_);
209 : :
210 [ + - + - : 1 : if VUNLIKELY (source >= current && source - current < size_) {
+ - ]
211 : 1 : return false;
212 : : }
213 : :
214 : 0 : Bytes::bytes_free(data_, size_);
215 : : }
216 : :
217 : 7 : header = target.header;
218 : :
219 : 7 : update_time_ns_ = target.update_time_ns_;
220 : 7 : std::memcpy(map_id_, target.map_id_, sizeof(map_id_));
221 : 7 : channel_ = target.channel_;
222 : 7 : freq_ = target.freq_;
223 : 7 : width_ = target.width_;
224 : 7 : height_ = target.height_;
225 : 7 : valid_cell_count_ = target.valid_cell_count_;
226 : 7 : resolution_ = target.resolution_;
227 : 7 : origin_x_ = target.origin_x_;
228 : 7 : origin_y_ = target.origin_y_;
229 : 7 : origin_z_ = target.origin_z_;
230 : 7 : origin_yaw_ = target.origin_yaw_;
231 : 7 : value_min_ = target.value_min_;
232 : 7 : value_max_ = target.value_max_;
233 : 7 : default_value_ = target.default_value_;
234 : 7 : occupied_threshold_ = target.occupied_threshold_;
235 : 7 : free_threshold_ = target.free_threshold_;
236 : 7 : cell_type_ = target.cell_type_;
237 : 7 : reserved_buf_ = target.reserved_buf_;
238 : 7 : reserved_buf2_ = target.reserved_buf2_;
239 : 7 : reserved_buf3_ = target.reserved_buf3_;
240 : 7 : is_owner_ = false;
241 : 7 : data_ = target.data_;
242 : 7 : size_ = target.size_;
243 : :
244 : 7 : return true;
245 : : }
246 : :
247 : 6 : bool OccupancyGrid::deep_copy(const OccupancyGrid& target) noexcept {
248 [ + + + - : 6 : if VLIKELY (data_ && is_owner_ && target.data_ && size_ != 0 && size_ == target.size_) {
+ + + - +
+ + - + +
+ - + + ]
249 : 3 : const auto current = reinterpret_cast<uintptr_t>(data_);
250 : 3 : const auto source = reinterpret_cast<uintptr_t>(target.data_);
251 : :
252 [ + + + - : 3 : if VUNLIKELY (source >= current && source - current < size_) {
+ + ]
253 : 2 : return false;
254 : : }
255 : :
256 : 1 : header = target.header;
257 : :
258 : 1 : update_time_ns_ = target.update_time_ns_;
259 : 1 : std::memcpy(map_id_, target.map_id_, sizeof(map_id_));
260 : 1 : channel_ = target.channel_;
261 : 1 : freq_ = target.freq_;
262 : 1 : width_ = target.width_;
263 : 1 : height_ = target.height_;
264 : 1 : valid_cell_count_ = target.valid_cell_count_;
265 : 1 : resolution_ = target.resolution_;
266 : 1 : origin_x_ = target.origin_x_;
267 : 1 : origin_y_ = target.origin_y_;
268 : 1 : origin_z_ = target.origin_z_;
269 : 1 : origin_yaw_ = target.origin_yaw_;
270 : 1 : value_min_ = target.value_min_;
271 : 1 : value_max_ = target.value_max_;
272 : 1 : default_value_ = target.default_value_;
273 : 1 : occupied_threshold_ = target.occupied_threshold_;
274 : 1 : free_threshold_ = target.free_threshold_;
275 : 1 : cell_type_ = target.cell_type_;
276 : 1 : reserved_buf_ = target.reserved_buf_;
277 : 1 : reserved_buf2_ = target.reserved_buf2_;
278 : 1 : reserved_buf3_ = target.reserved_buf3_;
279 : :
280 : 1 : std::memcpy(data_, target.data_, size_);
281 : :
282 : 1 : return true;
283 : : }
284 : :
285 [ - + ]: 3 : if VUNLIKELY (!shallow_copy(target)) {
286 : 0 : return false;
287 : : }
288 : :
289 [ + - + - ]: 3 : if (data_ && size_ != 0) {
290 : 3 : auto* target_data = data_;
291 : 3 : data_ = Bytes::bytes_malloc(size_);
292 : :
293 [ - + ]: 3 : if VUNLIKELY (!data_) {
294 : 0 : size_ = 0;
295 : 0 : return false;
296 : : }
297 : :
298 : 3 : std::memcpy(data_, target_data, size_);
299 : 3 : is_owner_ = true;
300 : : }
301 : :
302 : 3 : return true;
303 : : }
304 : :
305 : 4 : bool OccupancyGrid::move_copy(OccupancyGrid& target) noexcept {
306 [ + + ]: 4 : if VUNLIKELY (!shallow_copy(target)) {
307 : 1 : return false;
308 : : }
309 : :
310 : 3 : is_owner_ = target.is_owner_;
311 : :
312 : 3 : target.update_time_ns_ = 0;
313 : 3 : std::memset(target.map_id_, 0, sizeof(target.map_id_));
314 : 3 : target.channel_ = 0;
315 : 3 : target.freq_ = 0;
316 : 3 : target.width_ = 0;
317 : 3 : target.height_ = 0;
318 : 3 : target.valid_cell_count_ = 0;
319 : 3 : target.resolution_ = 0;
320 : 3 : target.origin_x_ = 0;
321 : 3 : target.origin_y_ = 0;
322 : 3 : target.origin_z_ = 0;
323 : 3 : target.origin_yaw_ = 0;
324 : 3 : target.value_min_ = 0;
325 : 3 : target.value_max_ = 0;
326 : 3 : target.default_value_ = 0;
327 : 3 : target.occupied_threshold_ = 0;
328 : 3 : target.free_threshold_ = 0;
329 : 3 : target.cell_type_ = kCellUnknown;
330 : 3 : target.reserved_buf_ = 0;
331 : 3 : target.reserved_buf2_ = 0;
332 : 3 : target.reserved_buf3_ = 0;
333 : 3 : target.is_owner_ = false;
334 : 3 : target.data_ = nullptr;
335 : 3 : target.size_ = 0;
336 : :
337 : : #if defined(__GNUC__) && !defined(__clang__)
338 : : #pragma GCC diagnostic push
339 : : #pragma GCC diagnostic ignored "-Wclass-memaccess"
340 : : #if __GNUC__ >= 11
341 : : #pragma GCC diagnostic ignored "-Wstringop-overread"
342 : : #endif
343 : : #endif
344 : :
345 : 3 : std::memset(&target.header, 0, sizeof(header));
346 : :
347 : : #if defined(__GNUC__) && !defined(__clang__)
348 : : #pragma GCC diagnostic pop
349 : : #endif
350 : :
351 : 3 : return true;
352 : : }
353 : :
354 : 37 : bool OccupancyGrid::create(size_t _size) noexcept {
355 [ + + ]: 37 : if VUNLIKELY (_size == 0) {
356 : 1 : return false;
357 : : }
358 : :
359 [ + + + - : 36 : if (is_owner_ && data_ && size_ != 0) {
+ - ]
360 : 2 : Bytes::bytes_free(data_, size_);
361 : : }
362 : :
363 : 36 : data_ = Bytes::bytes_malloc(_size);
364 : :
365 [ - + ]: 36 : if VUNLIKELY (!data_) {
366 : 0 : size_ = 0;
367 : 0 : is_owner_ = false;
368 : 0 : return false;
369 : : }
370 : :
371 : 36 : size_ = _size;
372 : 36 : is_owner_ = true;
373 : :
374 : 36 : return true;
375 : : }
376 : :
377 : 5 : void OccupancyGrid::clear() noexcept {
378 [ + + + - : 5 : if (is_owner_ && data_ && size_ != 0) {
+ - ]
379 : 4 : Bytes::bytes_free(data_, size_);
380 : : }
381 : :
382 : 5 : update_time_ns_ = 0;
383 : 5 : std::memset(map_id_, 0, sizeof(map_id_));
384 : 5 : channel_ = 0;
385 : 5 : freq_ = 0;
386 : 5 : width_ = 0;
387 : 5 : height_ = 0;
388 : 5 : valid_cell_count_ = 0;
389 : 5 : resolution_ = 0;
390 : 5 : origin_x_ = 0;
391 : 5 : origin_y_ = 0;
392 : 5 : origin_z_ = 0;
393 : 5 : origin_yaw_ = 0;
394 : 5 : value_min_ = 0;
395 : 5 : value_max_ = 0;
396 : 5 : default_value_ = 0;
397 : 5 : occupied_threshold_ = 0;
398 : 5 : free_threshold_ = 0;
399 : 5 : cell_type_ = kCellUnknown;
400 : 5 : is_owner_ = false;
401 : 5 : data_ = nullptr;
402 : 5 : size_ = 0;
403 : :
404 : : #if defined(__GNUC__) && !defined(__clang__)
405 : : #pragma GCC diagnostic push
406 : : #pragma GCC diagnostic ignored "-Wclass-memaccess"
407 : : #if __GNUC__ >= 11
408 : : #pragma GCC diagnostic ignored "-Wstringop-overread"
409 : : #endif
410 : : #endif
411 : :
412 : 5 : std::memset(&header, 0, sizeof(header));
413 : :
414 : : #if defined(__GNUC__) && !defined(__clang__)
415 : : #pragma GCC diagnostic pop
416 : : #endif
417 : 5 : }
418 : :
419 : 7 : bool OccupancyGrid::shallow_copy(uint8_t* data, size_t size) noexcept {
420 [ + + + + : 7 : if VUNLIKELY (!data || size == 0) {
+ + ]
421 : 2 : return false;
422 : : }
423 : :
424 [ + + ]: 5 : if VUNLIKELY (data_ == data) {
425 : 1 : return false;
426 : : }
427 : :
428 [ + + + - : 4 : if (is_owner_ && data_ && size_ != 0) {
+ - ]
429 : 1 : const auto current = reinterpret_cast<uintptr_t>(data_);
430 : 1 : const auto source = reinterpret_cast<uintptr_t>(data);
431 : :
432 [ + - + - : 1 : if VUNLIKELY (source >= current && source - current < size_) {
+ - ]
433 : 1 : return false;
434 : : }
435 : :
436 : 0 : Bytes::bytes_free(data_, size_);
437 : : }
438 : :
439 : 3 : is_owner_ = false;
440 : :
441 : 3 : data_ = data;
442 : 3 : size_ = size;
443 : :
444 : 3 : return true;
445 : : }
446 : :
447 : 10 : bool OccupancyGrid::deep_copy(uint8_t* data, size_t size) noexcept {
448 [ + + + + : 10 : if VUNLIKELY (!data || size == 0) {
+ + ]
449 : 4 : return false;
450 : : }
451 : :
452 [ + + ]: 6 : if (is_owner_) {
453 [ - + ]: 4 : if VUNLIKELY (!data_) {
454 : : return false; // LCOV_EXCL_LINE GCOVR_EXCL_LINE
455 : : }
456 : :
457 : 4 : const auto current = reinterpret_cast<uintptr_t>(data_);
458 : 4 : const auto source = reinterpret_cast<uintptr_t>(data);
459 : :
460 [ + + + + : 4 : if VUNLIKELY (source >= current && source - current < size_) {
+ + ]
461 : 2 : return false;
462 : : }
463 : :
464 [ + + - + : 2 : if VUNLIKELY (size_ != size && !create(size)) {
- + ]
465 : 0 : return false;
466 : : }
467 [ - + ]: 2 : } else if VUNLIKELY (!create(size)) {
468 : 0 : return false;
469 : : }
470 : :
471 : 4 : std::memcpy(data_, data, size);
472 : :
473 : 4 : return true;
474 : : }
475 : :
476 : 4 : bool OccupancyGrid::fill_data(uint8_t* data, size_t size) noexcept { return deep_copy(data, size); }
477 : :
478 : 4 : uint64_t OccupancyGrid::update_time_ns() const noexcept { return update_time_ns_; }
479 : :
480 : 9 : std::string_view OccupancyGrid::map_id() const noexcept { return {map_id_, ::strnlen(map_id_, sizeof(map_id_))}; }
481 : :
482 : 3 : uint32_t OccupancyGrid::channel() const noexcept { return channel_; }
483 : :
484 : 3 : uint32_t OccupancyGrid::freq() const noexcept { return freq_; }
485 : :
486 : 14 : uint32_t OccupancyGrid::width() const noexcept { return width_; }
487 : :
488 : 12 : uint32_t OccupancyGrid::height() const noexcept { return height_; }
489 : :
490 : 3 : uint32_t OccupancyGrid::valid_cell_count() const noexcept { return valid_cell_count_; }
491 : :
492 : 4 : float OccupancyGrid::resolution() const noexcept { return resolution_; }
493 : :
494 : 3 : float OccupancyGrid::origin_x() const noexcept { return origin_x_; }
495 : :
496 : 3 : float OccupancyGrid::origin_y() const noexcept { return origin_y_; }
497 : :
498 : 3 : float OccupancyGrid::origin_z() const noexcept { return origin_z_; }
499 : :
500 : 3 : float OccupancyGrid::origin_yaw() const noexcept { return origin_yaw_; }
501 : :
502 : 3 : float OccupancyGrid::value_min() const noexcept { return value_min_; }
503 : :
504 : 3 : float OccupancyGrid::value_max() const noexcept { return value_max_; }
505 : :
506 : 4 : int32_t OccupancyGrid::default_value() const noexcept { return default_value_; }
507 : :
508 : 4 : float OccupancyGrid::occupied_threshold() const noexcept { return occupied_threshold_; }
509 : :
510 : 4 : float OccupancyGrid::free_threshold() const noexcept { return free_threshold_; }
511 : :
512 : 10 : OccupancyGrid::CellType OccupancyGrid::cell_type() const noexcept { return cell_type_; }
513 : :
514 : 12 : uint8_t OccupancyGrid::cell_size() const noexcept { return cell_size_of(cell_type_); }
515 : :
516 : 42 : const uint8_t* OccupancyGrid::data() const noexcept { return data_; }
517 : :
518 : 34 : size_t OccupancyGrid::size() const noexcept { return size_; }
519 : :
520 : 23 : bool OccupancyGrid::is_owner() const noexcept { return is_owner_; }
521 : :
522 : 3 : void OccupancyGrid::set_update_time_ns(uint64_t update_time_ns) noexcept { update_time_ns_ = update_time_ns; }
523 : :
524 : 8 : void OccupancyGrid::set_map_id(std::string_view map_id) noexcept {
525 : 8 : std::memset(map_id_, 0, sizeof(map_id_));
526 : :
527 : 8 : size_t copy_size = map_id.size();
528 : :
529 [ + + ]: 8 : if (copy_size >= sizeof(map_id_)) {
530 : 1 : copy_size = sizeof(map_id_) - 1;
531 : : }
532 : :
533 [ + + ]: 8 : if VLIKELY (copy_size != 0) {
534 : 7 : std::memcpy(map_id_, map_id.data(), copy_size);
535 : : }
536 : 8 : }
537 : :
538 : 2 : void OccupancyGrid::set_channel(uint32_t channel) noexcept { channel_ = channel; }
539 : :
540 : 2 : void OccupancyGrid::set_freq(uint32_t freq) noexcept { freq_ = freq; }
541 : :
542 : 10 : void OccupancyGrid::set_width(uint32_t width) noexcept { width_ = width; }
543 : :
544 : 10 : void OccupancyGrid::set_height(uint32_t height) noexcept { height_ = height; }
545 : :
546 : 2 : void OccupancyGrid::set_valid_cell_count(uint32_t valid_cell_count) noexcept { valid_cell_count_ = valid_cell_count; }
547 : :
548 : 3 : void OccupancyGrid::set_resolution(float resolution) noexcept { resolution_ = resolution; }
549 : :
550 : 3 : void OccupancyGrid::set_origin_x(float origin_x) noexcept { origin_x_ = origin_x; }
551 : :
552 : 3 : void OccupancyGrid::set_origin_y(float origin_y) noexcept { origin_y_ = origin_y; }
553 : :
554 : 2 : void OccupancyGrid::set_origin_z(float origin_z) noexcept { origin_z_ = origin_z; }
555 : :
556 : 3 : void OccupancyGrid::set_origin_yaw(float origin_yaw) noexcept { origin_yaw_ = origin_yaw; }
557 : :
558 : 3 : void OccupancyGrid::set_value_min(float value_min) noexcept { value_min_ = value_min; }
559 : :
560 : 3 : void OccupancyGrid::set_value_max(float value_max) noexcept { value_max_ = value_max; }
561 : :
562 : 3 : void OccupancyGrid::set_default_value(int32_t default_value) noexcept { default_value_ = default_value; }
563 : :
564 : 3 : void OccupancyGrid::set_occupied_threshold(float occupied_threshold) noexcept {
565 : 3 : occupied_threshold_ = occupied_threshold;
566 : 3 : }
567 : :
568 : 3 : void OccupancyGrid::set_free_threshold(float free_threshold) noexcept { free_threshold_ = free_threshold; }
569 : :
570 : 14 : void OccupancyGrid::set_cell_type(CellType cell_type) noexcept { cell_type_ = cell_type; }
571 : :
572 : 17 : uint8_t OccupancyGrid::cell_size_of(CellType type) noexcept {
573 : 17 : uint8_t target_size = 0;
574 : :
575 [ + + + + ]: 17 : if (type == kCellInt8 || type == kCellUint8) {
576 : 3 : target_size = 1;
577 [ + + ]: 14 : } else if (type == kCellUint16) {
578 : 9 : target_size = 2;
579 [ + + ]: 5 : } else if (type == kCellFloat32) {
580 : 2 : target_size = 4;
581 : : }
582 : :
583 : 17 : return target_size;
584 : : }
585 : :
586 : : } // namespace zerocopy
587 : :
588 : : } // namespace vlink
|
