23 #include "sick_tim55x_common_aqt.h"
25 #include <core/threading/mutex.h>
26 #include <core/threading/mutex_locker.h>
27 #include <utils/math/angle.h>
28 #include <utils/misc/string_split.h>
75 std::string &cfg_prefix)
79 set_name(
"SickTiM55x(%s)", cfg_name.c_str());
80 pre_init_done_ =
false;
95 pre_init_done_ =
true;
98 throw Exception(
"LaserSick5xx: model has not yet been determined");
104 expected_num_data_ = 271;
108 expected_num_data_ = 811;
123 cfg_time_offset_ = 0.;
139 const char *req_scan_data =
"\x02sEN LMDscandata 0\x03";
146 std::string rep_dev_indent;
148 const char *req_dev_indent =
"\x02sRI0\x03\0";
152 e.
append(
"Failed to get device indent");
155 rep_dev_indent +=
'\0';
156 rep_dev_indent = rep_dev_indent.substr(9, rep_dev_indent.length() - 11);
157 dev_model_ = rep_dev_indent.substr(0, rep_dev_indent.find(
" "));
161 const char *req_scan_data =
"\x02sEN LMDscandata 1\x03";
165 e.
append(
"Failed to start data streaming");
178 const char *req_scan_data =
"\x02sEN LMDscandata 0\x03";
187 const char *req_scan_data =
"\x02sEN LMDscandata 1\x03";
201 size_t datagram_length)
203 static const size_t HEADER_FIELDS = 33;
205 std::string datagram_s((
const char *)datagram, datagram_length);
206 std::vector<std::string> fields = str_split(datagram_s,
' ');
208 size_t count = fields.size();
213 if (count < HEADER_FIELDS) {
214 throw Exception(
"Insufficient number of fields received");
216 if (fields[15] !=
"0") {
217 throw Exception(
"Invalid datagram format, ignoring scan");
219 if (fields[20] !=
"DIST1") {
220 throw Exception(
"Invalid datagram format (DIST1), ignoring scan");
225 unsigned short int number_of_data = 0;
226 sscanf(fields[25].c_str(),
"%hx", &number_of_data);
228 if (number_of_data != expected_num_data_) {
229 throw Exception(
"Invalid data length, got %u, expected %u", number_of_data, expected_num_data_);
231 if (count < HEADER_FIELDS + number_of_data) {
232 throw Exception(
"Invalid number of fields received, got %zu, expected %u+%u=%u",
236 HEADER_FIELDS + number_of_data);
240 size_t rssi_idx = 26 + number_of_data;
242 sscanf(fields[rssi_idx].c_str(),
"%d", &tmp);
244 unsigned short int number_of_rssi_data = 0;
246 sscanf(fields[rssi_idx + 6].c_str(),
"%hx", &number_of_rssi_data);
249 if (number_of_rssi_data != number_of_data) {
250 throw Exception(
"Number of RSSI data is lower than number of range data (%d vs %d)",
252 number_of_rssi_data);
257 if (count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6) {
258 throw Exception(
"Less fields than expected for %d data points (%zu)", number_of_data, count);
261 if (fields[rssi_idx + 1] !=
"RSSI1") {
262 throw Exception(
"Field %zu of received data is not equal to RSSI1 (%s)",
264 fields[rssi_idx + 1].c_str());
284 unsigned short scanning_freq = -1;
285 sscanf(fields[16].c_str(),
"%hx", &scanning_freq);
286 float scan_time = 1.0 / (scanning_freq / 100.0);
306 unsigned int starting_angle_hexval = 0;
307 sscanf(fields[23].c_str(),
"%x", &starting_angle_hexval);
308 int starting_angle_val =
static_cast<int>(starting_angle_hexval);
309 float angle_min = (starting_angle_val / 10000.0) / 180.0 * M_PI - M_PI / 2;
312 unsigned short angular_step_width = -1;
313 sscanf(fields[24].c_str(),
"%hx", &angular_step_width);
314 float angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
315 float angle_increment_deg =
rad2deg(angle_increment);
325 int start_idx = (int)roundf(
rad2deg(angle_min) / angle_increment_deg);
327 for (
int j = 0; j < number_of_data; ++j) {
328 unsigned short range;
329 sscanf(fields[j + 26].c_str(),
"%hx", &range);
348 size_t offset = 26 + number_of_data + 7;
349 for (
int j = 0; j < number_of_data; ++j) {
350 unsigned short intensity;
351 sscanf(fields[j + offset].c_str(),
"%hx", &intensity);
359 float time_increment = scan_time * angle_increment / (2.0 * M_PI);
361 *
_timestamp -= (double)number_of_data * time_increment;
375 SickTiM55xCommonAcquisitionThread::config_value_changed(
383 SickTiM55xCommonAcquisitionThread::config_value_erased(
const char *path)