23 #include "sick_tim55x_common_aqt.h" 25 #include <core/threading/mutex.h> 26 #include <core/threading/mutex_locker.h> 27 #include <utils/misc/string_split.h> 28 #include <utils/math/angle.h> 77 std::string &cfg_prefix)
80 set_name(
"SickTiM55x(%s)", cfg_name.c_str());
81 pre_init_done_ =
false;
96 if (pre_init_done_)
return;
97 pre_init_done_ =
true;
100 throw Exception(
"LaserSick5xx: model has not yet been determined");
105 expected_num_data_ = 271;
108 expected_num_data_ = 811;
120 cfg_time_offset_ = 0.;
135 const char *req_scan_data =
"\x02sEN LMDscandata 0\x03";
141 std::string rep_dev_indent;
143 const char *req_dev_indent =
"\x02sRI0\x03\0";
147 e.
append(
"Failed to get device indent");
150 rep_dev_indent +=
'\0';
151 rep_dev_indent = rep_dev_indent.substr(9, rep_dev_indent.length() - 11);
152 dev_model_ = rep_dev_indent.substr(0, rep_dev_indent.find(
" "));
156 const char *req_scan_data =
"\x02sEN LMDscandata 1\x03";
160 e.
append(
"Failed to start data streaming");
174 const char *req_scan_data =
"\x02sEN LMDscandata 0\x03";
182 const char *req_scan_data =
"\x02sEN LMDscandata 1\x03";
196 size_t datagram_length)
198 static const size_t HEADER_FIELDS = 33;
200 std::string datagram_s((
const char *)datagram, datagram_length);
201 std::vector<std::string> fields = str_split(datagram_s,
' ');
203 size_t count = fields.size();
208 if (count < HEADER_FIELDS) {
209 throw Exception(
"Insufficient number of fields received");
211 if (fields[15] !=
"0") {
212 throw Exception(
"Invalid datagram format, ignoring scan");
214 if (fields[20] !=
"DIST1") {
215 throw Exception(
"Invalid datagram format (DIST1), ignoring scan");
220 unsigned short int number_of_data = 0;
221 sscanf(fields[25].c_str(),
"%hx", &number_of_data);
223 if (number_of_data != expected_num_data_) {
224 throw Exception(
"Invalid data length, got %u, expected %u",
225 number_of_data, expected_num_data_);
227 if (count < HEADER_FIELDS + number_of_data) {
228 throw Exception(
"Invalid number of fields received, got %zu, expected %u+%u=%u",
229 count, HEADER_FIELDS, number_of_data, HEADER_FIELDS + number_of_data);
233 size_t rssi_idx = 26 + number_of_data;
235 sscanf(fields[rssi_idx].c_str(),
"%d", &tmp);
237 unsigned short int number_of_rssi_data = 0;
239 sscanf(fields[rssi_idx + 6].c_str(),
"%hx", &number_of_rssi_data);
242 if (number_of_rssi_data != number_of_data) {
243 throw Exception(
"Number of RSSI data is lower than number of range data (%d vs %d)",
244 number_of_data, number_of_rssi_data);
249 if (count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6) {
250 throw Exception(
"Less fields than expected for %d data points (%zu)",
251 number_of_data, count);
254 if (fields[rssi_idx + 1] !=
"RSSI1") {
255 throw Exception(
"Field %zu of received data is not equal to RSSI1 (%s)",
256 rssi_idx + 1, fields[rssi_idx + 1].c_str());
276 unsigned short scanning_freq = -1;
277 sscanf(fields[16].c_str(),
"%hx", &scanning_freq);
278 float scan_time = 1.0 / (scanning_freq / 100.0);
298 int starting_angle_val = -1;
299 sscanf(fields[23].c_str(),
"%x", &starting_angle_val);
300 float angle_min = (starting_angle_val / 10000.0) / 180.0 * M_PI - M_PI / 2;
303 unsigned short angular_step_width = -1;
304 sscanf(fields[24].c_str(),
"%hx", &angular_step_width);
305 float angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
306 float angle_increment_deg =
rad2deg(angle_increment);
316 int start_idx = (int)roundf(
rad2deg(angle_min) / angle_increment_deg);
318 for (
int j = 0; j < number_of_data; ++j) {
319 unsigned short range;
320 sscanf(fields[j + 26].c_str(),
"%hx", &range);
339 size_t offset = 26 + number_of_data + 7;
340 for (
int j = 0; j < number_of_data; ++j) {
341 unsigned short intensity;
342 sscanf(fields[j + offset].c_str(),
"%hx", &intensity);
350 float time_increment = scan_time * angle_increment / (2.0 * M_PI);
352 *
_timestamp -= number_of_data * time_increment;
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.
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.
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.
void read_common_config()
Read common configuration parameters.
virtual void flush_device()=0
Flush the device.
void lock()
Lock this mutex.
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.