26 {
27 if (inited_) {
28 return 0;
29 }
30 for (auto& packet : scan_msgs->firing_pkts()) {
31 if (packet.data().size() < 1206) {
32 AERROR <<
"Ivalid packet data size, expect 1206, actually "
33 << packet.data().size();
34 return -1;
35 }
36 uint8_t* data =
37 reinterpret_cast<uint8_t*>(const_cast<char*>(packet.data().c_str()));
38 status_types_.emplace_back(data[1204]);
39 status_values_.emplace_back(data[1205]);
40 }
41
42 if (status_types_.size() < 5789 * 2) {
43 AINFO <<
"Wait for more scan msgs";
44 return -1;
45 }
47 int unit_size = static_cast<int>(unit_indexs_.size());
48 if (unit_size < 2) {
49
50 AINFO <<
"unit count less than 2, maybe lost packets";
51 return -1;
52 }
53
54 if (unit_indexs_[unit_size - 1] - unit_indexs_[unit_size - 2] !=
55 65 * 64) {
56
57 AERROR <<
"two unit distance is wrong";
58 return -1;
59 }
60
61 int start_index = unit_indexs_[unit_size - 2];
62 for (int i = 0; i < 64; ++i) {
63 LaserCorrection laser_correction;
64
65 int index_16 = start_index + i * 64 + 16;
66 int index_32 = start_index + i * 64 + 32;
67 int index_48 = start_index + i * 64 + 48;
68
69 laser_correction.laser_ring = status_values_[index_16];
70
71 laser_correction.vert_correction =
72 *reinterpret_cast<int16_t*>(&status_values_[index_16 + 1]) / 100.0f *
74 laser_correction.rot_correction =
75 *reinterpret_cast<int16_t*>(&status_values_[index_16 + 3]) / 100.0f *
77 laser_correction.dist_correction =
78 *reinterpret_cast<int16_t*>(&status_values_[index_16 + 5]) / 10.0f /
79 100.0f;
80 laser_correction.dist_correction_x =
81 *reinterpret_cast<int16_t*>(&status_values_[index_32]) / 10.0f /
82 100.0f;
83 laser_correction.dist_correction_y =
84 *reinterpret_cast<int16_t*>(&status_values_[index_32 + 2]) / 10.0f /
85 100.0f;
86 laser_correction.vert_offset_correction =
87 *reinterpret_cast<int16_t*>(&status_values_[index_32 + 4]) / 10.0f /
88 100.0f;
89 laser_correction.horiz_offset_correction =
90 static_cast<int16_t>(static_cast<int16_t>(status_values_[index_48])
91 << 8 |
92 status_values_[index_32 + 6]) /
93 10.0f / 100.0f;
94 laser_correction.focal_distance =
95 *reinterpret_cast<int16_t*>(&status_values_[index_48 + 1]) / 10.0f /
96 100.0f;
97 laser_correction.focal_slope =
98 *reinterpret_cast<int16_t*>(&status_values_[index_48 + 3]) /
99 10.0f;
100 laser_correction.max_intensity = status_values_[index_48 + 6];
101 laser_correction.min_intensity = status_values_[index_48 + 5];
102
103 laser_correction.cos_rot_correction = cosf(laser_correction.rot_correction);
104 laser_correction.sin_rot_correction = sinf(laser_correction.rot_correction);
105 laser_correction.cos_vert_correction =
106 cosf(laser_correction.vert_correction);
107 laser_correction.sin_vert_correction =
108 sinf(laser_correction.vert_correction);
109 laser_correction.focal_offset =
110 256.0f * static_cast<float>(
111 std::pow(1 - laser_correction.focal_distance / 13100, 2));
112
114 laser_correction;
115 }
118 inited_ = true;
119 return 0;
120}
std::map< int, LaserCorrection > laser_corrections_
constexpr double DEGRESS_TO_RADIANS