37 {
38
39 out_msg->mutable_header()->set_timestamp_sec(
40 scan_msg->basetime() / 1000000000.0);
41 out_msg->mutable_header()->set_module_name(
42 scan_msg->header().module_name());
43 out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id());
44 out_msg->set_height(1);
45 out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0);
46 out_msg->mutable_header()->set_sequence_num(
47 scan_msg->header().sequence_num());
48 gps_base_usec_ = scan_msg->basetime();
49
50 double scan_altitude_original_degree33[32];
51 double scan_altitude_original_degree1[32];
52 int startpos = 0;
53
54 const unsigned char* difop_ptr
55 = (const unsigned char*)scan_msg->difop_pkts(0).data().c_str();
56 if (difop_ptr[0] == 0xa5 && difop_ptr[1] == 0xff && difop_ptr[2] == 0x00
57 && difop_ptr[3] == 0x5a) {
58 if ((difop_ptr[1202] >= 0x02 && difop_ptr[1203] >= 0x80)
59 || difop_ptr[1202] == 0x03) {
60 lslidar_type = 3;
61 startpos = 245;
62
63 adjust_angle
64 = (difop_ptr[186] * 256
65 + difop_ptr[187]);
66 if (adjust_angle > 32767) {
67 adjust_angle = adjust_angle - 65535;
68 }
69 adjust_angle_two
70 = (difop_ptr[190] * 256
71 + difop_ptr[191]);
72 if (adjust_angle_two > 32767) {
73 adjust_angle_two = adjust_angle_two - 65535;
74 }
75 adjust_angle_three
76 = (difop_ptr[188] * 256
77 + difop_ptr[189]);
78 if (adjust_angle_three > 32767) {
79 adjust_angle_three = adjust_angle_three - 65535;
80 }
81 adjust_angle_four
82 = (difop_ptr[192] * 256
83 + difop_ptr[193]);
84 if (adjust_angle_four > 32767) {
85 adjust_angle_four = adjust_angle_four - 65535;
86 }
87 memcpy(scan_altitude_original_degree1,
88 scan_altitude_original_A3,
89 32 * 8);
90 memcpy(scan_altitude_original_degree33,
91 scan_altitude_original_C3,
92 32 * 8);
93
94 if (difop_ptr[185] == 0 || difop_ptr[185] == 1) {
95 int return_mode = difop_ptr[185] + 1;
96 config_.set_return_mode(return_mode);
97
98 if (difop_ptr[1195] == 0x21)
100 else
102
103 config_.set_distance_unit(0.4f);
104
105 for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) {
106
109 static_cast<float>(
110 scan_altitude_original_A3[i] * M_PI)
111 / 180.0f);
113 static_cast<float>(
114 scan_altitude_original_A3[i] * M_PI)
115 / 180.0f);
116 scan_altitude_A[i] = scan_altitude_original_A3[i];
117 }
118
121 static_cast<float>(
122 scan_altitude_original_C3[i] * M_PI)
123 / 180.0f);
125 static_cast<float>(
126 scan_altitude_original_C3[i] * M_PI)
127 / 180.0f);
128 scan_altitude_C[i] = scan_altitude_original_C3[i];
129 }
130 }
131 }
132 } else {
133 lslidar_type = 2;
134 startpos = 882;
135
136 adjust_angle
137 = (difop_ptr[34] * 256
138 + difop_ptr[35]);
139 if (adjust_angle > 32767) {
140 adjust_angle = adjust_angle - 65535;
141 }
142 adjust_angle_two
143 = (difop_ptr[42] * 256
144 + difop_ptr[43]);
145 if (adjust_angle_two > 32767) {
146 adjust_angle_two = adjust_angle_two - 65535;
147 }
148 adjust_angle_three
149 = (difop_ptr[66] * 256
150 + difop_ptr[67]);
151 if (adjust_angle_three > 32767) {
152 adjust_angle_three = adjust_angle_three - 65535;
153 }
154 adjust_angle_four
155 = (difop_ptr[68] * 256
156 + difop_ptr[69]);
157 if (adjust_angle_four > 32767) {
158 adjust_angle_four = adjust_angle_four - 65535;
159 }
160 AWARN <<
"Load config failed, config file";
161
162 for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) {
163
166 static_cast<float>(
167 scan_altitude_original_A[i] * M_PI)
168 / 180.0f);
170 static_cast<float>(
171 scan_altitude_original_A[i] * M_PI)
172 / 180.0f);
173 scan_altitude_A[i] = scan_altitude_original_A[i];
174 }
175
178 static_cast<float>(
179 scan_altitude_original_C[i] * M_PI)
180 / 180.0f);
182 static_cast<float>(
183 scan_altitude_original_C[i] * M_PI)
184 / 180.0f);
185 scan_altitude_C[i] = scan_altitude_original_C[i];
186 }
187 }
188 memcpy(scan_altitude_original_degree1,
189 scan_altitude_original_A,
190 32 * 8);
191 memcpy(scan_altitude_original_degree33,
192 scan_altitude_original_C,
193 32 * 8);
194 }
195 }
196
197
199 for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) {
200 uint8_t data1 = difop_ptr[startpos + 2 * i];
201 uint8_t data2 = difop_ptr[startpos + 2 * i + 1];
202 int vert_angle = data1 * 256 + data2;
203 if (vert_angle > 32767) {
204 vert_angle = vert_angle - 65535;
205 }
206
208 scan_altitude_A[i]
209 = static_cast<double>(vert_angle * ROTATION_RESOLUTION);
210 if (fabs(scan_altitude_original_degree1[i] - scan_altitude_A[i])
211 > 1.5) {
212 scan_altitude_A[i] = scan_altitude_original_degree1[i];
213 }
215 }
216
218 scan_altitude_C[i]
219 = static_cast<double>(vert_angle * ROTATION_RESOLUTION);
220 if (fabs(scan_altitude_original_degree33[i]
221 - scan_altitude_C[i])
222 > 1.5) {
223 scan_altitude_C[i] = scan_altitude_original_degree33[i];
224 }
226 }
227 }
228 }
229
230 size_t packets_size = scan_msg->firing_pkts_size();
231 block_num = 0;
232 packet_number_ = packets_size;
233
234 AINFO <<
"packets_size :" << packets_size;
235
236 for (size_t i = 0; i < packets_size; ++i) {
237 Unpack(scan_msg->firing_pkts(static_cast<int>(i)), out_msg, i);
240 }
241
242 if (out_msg->point().empty()) {
243
245 }
246
247
248 out_msg->set_width(out_msg->point_size());
249}
double cos_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
double sin_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
optional uint32 degree_mode
optional bool config_vert