FOUND Coverage Report

src/
File: datafile/serialization.cpp
Date: 2025-11-20 21:57:26
Lines:
107/107
100.0%
Functions:
18/18
100.0%
Branches:
55/55
100.0%
Line Branch Exec Source
1 #include <memory>
2 #include <fstream>
3 #include <iostream>
4 #include <string>
5
6 #include "common/spatial/attitude-utils.hpp"
7 #include "common/logging.hpp"
8
9 #include "datafile/encoding.hpp"
10 #include "datafile/serialization.hpp"
11
12 namespace found {
13
14 24 void hton(DataFileHeader& header) {
15 24 header.version = htonl(header.version);
16 24 header.num_positions = htonl(header.num_positions);
17 24 header.crc = htonl(header.crc);
18 24 }
19
20 44 void ntoh(DataFileHeader& header) {
21 44 header.version = ntohl(header.version);
22 44 header.num_positions = ntohl(header.num_positions);
23 44 header.crc = ntohl(header.crc);
24 44 }
25
26 /**
27 *
28 * @brief Writes a decimal value to the given output stream in network byte order.
29 *
30 * @param stream The stream to write into
31 * @param value The value to read from
32 *
33 * @post stream has value written into it, in network byte order
34 */
35 162 inline void write(std::ostream& stream, const decimal& value) {
36 #ifdef FOUND_FLOAT_MODE
37 double v = htond(static_cast<double>(value));
38 #else
39 162 double v = htondec(value);
40 #endif
41
1/1
✓ Branch 1 taken 162 times.
 162 stream.write(reinterpret_cast<const char*>(&v), sizeof(double));
42 162 }
43
44 /**
45 *
46 * @brief Reads a decimal value from the given input stream.
47 *
48 * @param stream The stream to read from
49 * @param value The value to write into
50 *
51 * @post value is now the decimal value that is at the position
52 * where stream is, in host byte order
53 */
54 222 inline void read(std::istream& stream, decimal& value) {
55 #ifdef FOUND_FLOAT_MODE
56 double temp;
57 stream.read(reinterpret_cast<char*>(&temp), sizeof(double));
58 #else
59 222 stream.read(reinterpret_cast<char*>(&value), sizeof(double));
60 #endif
61
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 219 times.
 222 if (stream.gcount() != sizeof(double)) {
62
1/1
✓ Branch 4 taken 3 times.
 3 throw std::ios_base::failure("Failed to read decimal value");
63 }
64 #ifdef FOUND_FLOAT_MODE
65 value = DECIMAL(ntohd(temp));
66 #else
67 219 value = ntohdec(value);
68 #endif
69 219 }
70
71 /**
72 *
73 * @brief Writes a 64-bit unsigned integer to the given output stream in network byte order.
74 *
75 * @param stream The stream to write into
76 * @param value The value to read from
77 *
78 * @post stream has value written into it, in network byte order
79 *
80 */
81 22 inline void write(std::ostream& stream, const uint64_t& value) {
82 22 uint64_t v = htonl(value);
83
1/1
✓ Branch 1 taken 22 times.
 22 stream.write(reinterpret_cast<const char*>(&v), sizeof(uint64_t));
84 22 }
85
86 /**
87 *
88 * @brief Reads a 64-bit unsigned integer from the given input stream.
89 *
90 * @param stream The stream to read from
91 * @param value The value to write into
92 *
93 * @post value is now the integer value that is at the position
94 * where stream is, in host byte order
95 *
96 */
97 21 inline void read(std::istream& stream, uint64_t& value) {
98 21 stream.read(reinterpret_cast<char*>(&value), sizeof(uint64_t));
99
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 20 times.
 21 if (stream.gcount() != sizeof(uint64_t)) {
100
1/1
✓ Branch 4 taken 1 times.
 1 throw std::ios_base::failure("Failed to read uint64_t value");
101 }
102 20 value = ntohl(value);
103 20 }
104
105 /**
106 *
107 * @brief Writes a 32-bit unsigned integer to the given output stream in network byte order.
108 *
109 * @param stream The stream to write into
110 * @param value The value to read from
111 *
112 * @post stream has value written into it, in network byte order
113 *
114 */
115 inline void write(std::ostream& stream, const uint32_t& value) {
116 uint32_t v = htonl(value);
117 stream.write(reinterpret_cast<const char*>(&v), sizeof(uint32_t));
118 }
119
120 /**
121 *
122 * @brief Reads a 32-bit unsigned integer from the given input stream in network byte order.
123 *
124 * @param stream The stream to read from
125 * @param value The value to write into
126 *
127 * @post value is now the integer value that is at the position
128 * where stream is, in host byte order
129 *
130 */
131 inline void read(std::istream& stream, uint32_t& value) {
132 stream.read(reinterpret_cast<char*>(&value), sizeof(uint32_t));
133 if (stream.gcount() != sizeof(uint32_t)) {
134 throw std::ios_base::failure("Failed to read uint32_t value");
135 }
136 value = ntohl(value);
137 }
138
139 /**
140 *
141 * @brief Serializes an Quaternion to the given output stream.
142 *
143 * @param stream The stream to write into
144 * @param quat The angles to read from
145 *
146 * @post stream has all fields of quat written into it, each in network byte order.
147 *
148 */
149 24 inline void write(std::ostream& stream, const Quaternion& quat) {
150 24 write(stream, quat.real);
151 24 write(stream, quat.i);
152 24 write(stream, quat.j);
153 24 write(stream, quat.k);
154 24 }
155
156 /**
157 *
158 * @brief Reads Quaternion data from an input stream.
159 *
160 * @param stream The stream to read from
161 * @param quat The angles to write into
162 *
163 * @post quat has the values that stream held
164 *
165 */
166 42 inline void read(std::istream& stream, Quaternion& quat) {
167 42 read(stream, quat.real);
168 40 read(stream, quat.i);
169 40 read(stream, quat.j);
170 40 read(stream, quat.k);
171 39 }
172
173 /**
174 *
175 * @brief Serializes a Vec3 object to the given output stream.
176 *
177 * @param stream The stream to write into
178 * @param v The Vec3 to read from
179 *
180 * @post stream has all fields of v written into it, each in network byte order.
181 *
182 */
183 22 inline void write(std::ostream& stream, const Vec3& v) {
184 22 write(stream, v.x);
185 22 write(stream, v.y);
186 22 write(stream, v.z);
187 22 }
188
189 /**
190 *
191 * @brief Reads a Vec3 object from the given input stream.
192 *
193 * @param stream The stream to read from
194 * @param v The Vec3 to write into
195 *
196 * @post v has the values that stream held
197 *
198 */
199 20 inline void read(std::istream& stream, Vec3& v) {
200 20 read(stream, v.x);
201 20 read(stream, v.y);
202 20 read(stream, v.z);
203 20 }
204
205 /**
206 *
207 * @brief Serializes a LocationRecord object to the given output stream.
208 *
209 * @param stream The stream to read from
210 * @param record The record to write into
211 *
212 * @post record has the values that stream held
213 *
214 */
215 22 inline void write(std::ostream& stream, const LocationRecord& record) {
216 22 write(stream, record.timestamp);
217 22 write(stream, record.position);
218 22 }
219
220 /**
221 *
222 * @brief Reads data from the input stream into a LocationRecord object.
223 *
224 * @param stream The stream to read from
225 * @param record The LocationRecord to write into
226 *
227 * @post record has the values that stream held
228 *
229 */
230 21 inline void read(std::istream& stream, LocationRecord& record) {
231 21 read(stream, record.timestamp);
232 20 read(stream, record.position);
233 20 }
234
235 77 uint32_t calculateCRC32(const void* data, size_t length) {
236 77 uint32_t crc = 0xFFFFFFFFU;
237 77 const uint8_t* bytes = static_cast<const uint8_t*>(data);
238
2/2
✓ Branch 0 taken 924 times.
✓ Branch 1 taken 77 times.
 1001 for (size_t i = 0; i < length; ++i) {
239 924 crc ^= bytes[i];
240
2/2
✓ Branch 0 taken 7392 times.
✓ Branch 1 taken 924 times.
 8316 for (int j = 0; j < 8; ++j) {
241
2/2
✓ Branch 0 taken 3628 times.
✓ Branch 1 taken 3764 times.
 7392 if (crc & 1)
242 3628 crc = (crc >> 1) ^ 0xEDB88320U;
243 else
244 3764 crc = crc >> 1;
245 }
246 }
247 77 return crc ^ 0xFFFFFFFFU;
248 }
249
250 24 void serializeDataFile(const DataFile& data, std::ostream& stream) {
251 24 DataFileHeader header = data.header;
252 24 header.crc = calculateCRC32(&header, sizeof(header) - sizeof(header.crc));
253 24 hton(header);
254
1/1
✓ Branch 1 taken 24 times.
 24 stream.write(reinterpret_cast<const char*>(&header), sizeof(header));
255
256
1/1
✓ Branch 1 taken 24 times.
 24 write(stream, data.relative_attitude);
257
258
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 24 times.
 46 for (uint32_t i = 0; i < data.header.num_positions; ++i) {
259
1/1
✓ Branch 2 taken 22 times.
 22 write(stream, data.positions[i]);
260 }
261 24 }
262
263 44 DataFile deserializeDataFile(std::istream& stream) {
264 44 DataFile data;
265
1/1
✓ Branch 1 taken 42 times.
 44 data.header = readHeader(stream);
266
267
1/1
✓ Branch 1 taken 39 times.
 42 read(stream, data.relative_attitude);
268
269
1/1
✓ Branch 2 taken 39 times.
 39 data.positions = std::make_unique<LocationRecord[]>(data.header.num_positions);
270
2/2
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 38 times.
 59 for (uint32_t i = 0; i < data.header.num_positions; ++i) {
271
1/1
✓ Branch 2 taken 20 times.
 21 read(stream, data.positions[i]);
272 }
273
274 38 return data;
275 6 }
276
277 23 DataFile deserializeDataFile(std::istream& stream, const std::string &path) {
278 23 DataFile data = deserializeDataFile(stream);
279
1/1
✓ Branch 1 taken 21 times.
 21 data.path = path;
280
281 21 return data;
282 }
283
284
285 /**
286 * Validates the magic number in the header.
287 *
288 * @param magic The magic number to validate
289 *
290 * @return true iff magic == "FOUN"
291 */
292 48 bool isValidMagicNumber(const char magic[4]) {
293
8/8
✓ Branch 0 taken 47 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 46 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 45 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 44 times.
✓ Branch 7 taken 1 times.
 48 return magic[0] == 'F' && magic[1] == 'O' && magic[2] == 'U' && magic[3] == 'N';
294 }
295
296 51 DataFileHeader readHeader(std::istream& stream) {
297 51 DataFileHeader header;
298
1/1
✓ Branch 1 taken 51 times.
 51 stream.read(reinterpret_cast<char*>(&header), sizeof(DataFileHeader));
299
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 48 times.
 51 if (stream.gcount() != sizeof(header)) {
300
1/1
✓ Branch 4 taken 3 times.
 3 throw std::ios_base::failure("Failed to read header");
301 }
302
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 44 times.
 48 if (!isValidMagicNumber(header.magic)) {
303
1/1
✓ Branch 4 taken 4 times.
 4 throw std::ios_base::failure("Invalid magic number in header");
304 }
305
306 // Convert version and other fields from network to host byte order
307 44 ntoh(header);
308
309 // Validate CRC
310 44 uint32_t expected_crc = calculateCRC32(&header, sizeof(header) - sizeof(header.crc));
311
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 43 times.
 44 if (header.crc != expected_crc) {
312
11/11
✓ Branch 3 taken 1 times.
✓ Branch 10 taken 1 times.
✓ Branch 13 taken 1 times.
✓ Branch 16 taken 1 times.
✓ Branch 20 taken 1 times.
✓ Branch 23 taken 1 times.
✓ Branch 26 taken 1 times.
✓ Branch 29 taken 1 times.
✓ Branch 32 taken 1 times.
✓ Branch 36 taken 1 times.
✓ Branch 39 taken 1 times.
 3 LOG_ERROR("Expected CRC: " << expected_crc << ", Found CRC: " << ntohl(header.crc));
313
1/1
✓ Branch 4 taken 1 times.
 1 throw std::ios_base::failure("Header CRC validation failed: Corrupted file");
314 }
315
316 43 return header;
317 }
318
319 } // namespace found
320