-
Notifications
You must be signed in to change notification settings - Fork 1
/
packetmsg.hpp
120 lines (95 loc) · 3.66 KB
/
packetmsg.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
#ifndef PACKETMSG_HPP
#define PACKETMSG_HPP
#include "base64.hpp"
#include <string>
#include <vector>
class imu_packetmsg
{
public:
uint64_t imu_diag_time_ns;
uint64_t acc_read_time_ns;
uint64_t gyro_read_time_ns;
float acc_x;
float acc_y;
float acc_z;
float ang_vel_x;
float ang_vel_y;
float ang_vel_z;
imu_packetmsg() { }
imu_packetmsg(const std::string& message)
{
std::string decoded = base64_decode(message.substr(
message.find("\"buf\": \"") + 8, 68));
imu_diag_time_ns = (*(reinterpret_cast<const uint64_t*> (&decoded[0])));
acc_read_time_ns = (*(reinterpret_cast<const uint64_t*> (&decoded[8])));
gyro_read_time_ns = (*(reinterpret_cast<const uint64_t*>
(&decoded[16])));
acc_x = (*(reinterpret_cast<const float*> (&decoded[24])));
acc_y = (*(reinterpret_cast<const float*> (&decoded[28])));
acc_z = (*(reinterpret_cast<const float*> (&decoded[32])));
ang_vel_x = (*(reinterpret_cast<const float*> (&decoded[36])));
ang_vel_y = (*(reinterpret_cast<const float*> (&decoded[40])));
ang_vel_z = (*(reinterpret_cast<const float*> (&decoded[44])));
}
};
class lidar_packetmsg
{
public:
std::vector<uint64_t> timestamp;
std::vector<uint16_t> measurement_id;
std::vector<uint16_t> frame_id;
std::vector<uint32_t> encoder_count;
std::vector<std::vector<uint32_t>> range_mm;
std::vector<std::vector<uint16_t>> signal_photons;
std::vector<std::vector<uint16_t>> reflectivity;
std::vector<std::vector<uint16_t>> noise_photons;
std::vector<bool> azimuth_data_block_status;
lidar_packetmsg() { }
lidar_packetmsg(const std::string& message)
{
std::string decoded = base64_decode(message.substr(
message.find("\"buf\": \"") + 8, 16812));
timestamp.clear();
measurement_id.clear();
frame_id.clear();
encoder_count.clear();
range_mm.clear();
signal_photons.clear();
reflectivity.clear();
noise_photons.clear();
azimuth_data_block_status.clear();
for (size_t azimuth_block = 0; azimuth_block < 16; ++azimuth_block)
{
timestamp.push_back(*(reinterpret_cast<const uint64_t*>
(&decoded[(azimuth_block * 788) + 0])));
measurement_id.push_back(*(reinterpret_cast<const uint16_t*>
(&decoded[(azimuth_block * 788) + 8])));
frame_id.push_back(*(reinterpret_cast<const uint16_t*>
(&decoded[(azimuth_block * 788) + 10])));
encoder_count.push_back(*(reinterpret_cast<const uint32_t*>
(&decoded[(azimuth_block * 788) + 12])));
std::vector<uint32_t> block_range_mm;
std::vector<uint16_t> block_signal_photons;
std::vector<uint16_t> block_reflectivity;
std::vector<uint16_t> block_noise_photons;
for (size_t pixel = 0; pixel < 64; ++pixel)
{
block_range_mm.push_back(*(reinterpret_cast<const uint32_t*>
(&decoded[(azimuth_block * 788) + 16 + (pixel * 12)])));
block_reflectivity.push_back(*(reinterpret_cast<const uint16_t*>
(&decoded[(azimuth_block * 788) + 16 + (pixel * 12) + 4])));
block_signal_photons.push_back(*(reinterpret_cast<const uint16_t*>
(&decoded[(azimuth_block * 788) + 16 + (pixel * 12) + 6])));
block_noise_photons.push_back(*(reinterpret_cast<const uint16_t*>
(&decoded[(azimuth_block * 788) + 16 + (pixel * 12) + 8])));
}
range_mm.push_back(block_range_mm);
signal_photons.push_back(block_signal_photons);
reflectivity.push_back(block_reflectivity);
noise_photons.push_back(block_noise_photons);
azimuth_data_block_status.push_back(
!!decoded[(azimuth_block * 788) + 784]);
}
}
};
#endif // PACKETMSG_HPP