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)
virtual ~SickTiM55xCommonAcquisitionThread()
Destructor.
std::string cfg_name_
Name of the particular configuration instance.
virtual void open_device()=0
Open the device.
void resync()
Resynchronize to laser data.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Interface for configuration change handling.
void init_device()
Initialize device.
virtual void send_with_reply(const char *request, std::string *reply=NULL)=0
Send a request and expect a reply.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Logger * logger
This is the Logger member used to access the logger.
SickTiM55xCommonAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
void alloc_distances(unsigned int num_distances)
Allocate distances array.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
fawkes::Time * _timestamp
Time when the most recent data was received.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
void parse_datagram(const unsigned char *datagram, size_t datagram_length)
Parse incoming message from device.
virtual void close_device()=0
Close the device.
void set_name(const char *format,...)
Set name of thread.
Base class for exceptions in Fawkes.
unsigned int _distances_size
Assign this the size of the _distances array.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
const char * name() const
Get name of thread.
unsigned int _echoes_size
Assign this the size of the _echoes array.
bool _new_data
Set to true in your loop if new data is available.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Iterator interface to iterate over config values.
void alloc_echoes(unsigned int num_echoes)
Allocate echoes array.
void read_common_config()
Read common configuration parameters.
virtual void flush_device()=0
Flush the device.
void lock()
Lock this mutex.
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Time & stamp()
Set this time to the current time.
std::string dev_model_
Device model type as string.
Configuration * config
This is the Configuration member used to access the configuration.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
float * _echoes
Allocate a float array and copy your echo values here.
std::string cfg_prefix_
Configuration path prefix for this configuration.
void append(const char *format,...)
Append messages to the message list.