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();
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]);
42 if (status_types_.size() < 5789 * 2) {
43 AINFO <<
"Wait for more scan msgs";
47 int unit_size =
static_cast<int>(unit_indexs_.size());
50 AINFO <<
"unit count less than 2, maybe lost packets";
54 if (unit_indexs_[unit_size - 1] - unit_indexs_[unit_size - 2] !=
57 AERROR <<
"two unit distance is wrong";
61 int start_index = unit_indexs_[unit_size - 2];
62 for (
int i = 0; i < 64; ++i) {
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;
69 laser_correction.
laser_ring = status_values_[index_16];
72 *
reinterpret_cast<int16_t*
>(&status_values_[index_16 + 1]) / 100.0f *
75 *
reinterpret_cast<int16_t*
>(&status_values_[index_16 + 3]) / 100.0f *
78 *
reinterpret_cast<int16_t*
>(&status_values_[index_16 + 5]) / 10.0f /
81 *
reinterpret_cast<int16_t*
>(&status_values_[index_32]) / 10.0f /
84 *
reinterpret_cast<int16_t*
>(&status_values_[index_32 + 2]) / 10.0f /
87 *
reinterpret_cast<int16_t*
>(&status_values_[index_32 + 4]) / 10.0f /
90 static_cast<int16_t
>(
static_cast<int16_t
>(status_values_[index_48])
92 status_values_[index_32 + 6]) /
95 *
reinterpret_cast<int16_t*
>(&status_values_[index_48 + 1]) / 10.0f /
98 *
reinterpret_cast<int16_t*
>(&status_values_[index_48 + 3]) /
100 laser_correction.
max_intensity = status_values_[index_48 + 6];
101 laser_correction.
min_intensity = status_values_[index_48 + 5];
110 256.0f *
static_cast<float>(
123 int size =
static_cast<int>(status_values_.size());
126 if (unit_indexs_.size() > 0) {
127 start_index = unit_indexs_.back() + 5;
129 for (; start_index < size - 5; ++start_index) {
130 if (status_values_[start_index] == 85
131 && status_values_[start_index + 1] == 78
132 && status_values_[start_index + 2] == 73
133 && status_values_[start_index + 3] == 84
134 && status_values_[start_index + 4] == 35) {
135 unit_indexs_.emplace_back(start_index);
142 AERROR <<
"Please decode calibraion info first";
145 std::ofstream ofs(file_path.c_str(), std::ios::out);
146 ofs <<
"lasers:" << std::endl;
149 ofs <<
"dist_correction: " << correction.second.dist_correction <<
", ";
150 ofs <<
"dist_correction_x: " << correction.second.dist_correction_x <<
", ";
151 ofs <<
"dist_correction_y: " << correction.second.dist_correction_y <<
", ";
152 ofs <<
"focal_distance: " << correction.second.focal_distance <<
", ";
153 ofs <<
"focal_slope: " << correction.second.focal_slope <<
", ";
154 ofs <<
"horiz_offset_correction: "
155 << correction.second.horiz_offset_correction <<
", ";
156 ofs <<
"laser_id: " << correction.second.laser_ring <<
", ";
157 ofs <<
"max_intensity: " << correction.second.max_intensity <<
", ";
158 ofs <<
"min_intensity: " << correction.second.min_intensity <<
", ";
159 ofs <<
"rot_correction: " << correction.second.rot_correction <<
", ";
160 ofs <<
"vert_correction: " << correction.second.vert_correction <<
", ";
161 ofs <<
"vert_offset_correction: "
162 << correction.second.vert_offset_correction;
163 ofs <<
"}" << std::endl;
165 ofs <<
"num_lasers: " << calibration_.
num_lasers_ << std::endl;