Fawkes API  Fawkes Development Version
sick_tim55x_common_aqt.cpp
1 
2 /***************************************************************************
3  * sick_tim55x_aqt.cpp - Thread to retrieve laser data from Sick TiM55x
4  *
5  * Created: Tue Jun 10 16:53:23 2014
6  * Copyright 2008-2014 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "sick_tim55x_common_aqt.h"
24 
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>
29 
30 #include <cstdio>
31 #include <cstdlib>
32 #include <cstring>
33 #include <unistd.h>
34 
35 using namespace fawkes;
36 
37 /** @class SickTiM55xCommonAcquisitionThread "sick_tim55x_common_aqt.h"
38  * Laser acqusition thread for Sick TiM55x laser range finders.
39  * This thread fetches the data from the laser.
40  * @author Tim Niemueller
41  *
42  * @fn void SickTiM55xCommonAcquisitionThread::send_with_reply(const char *request, std::string *reply = NULL)
43  * Send a request and expect a reply.
44  * @param request request to send
45  * @param reply upon returns contains the received reply, maybe NULL to ignore the reply
46  *
47  * @fn void SickTiM55xCommonAcquisitionThread::open_device()
48  * Open the device.
49  * Virtual method implemented by the actual connection driver.
50  *
51  * @fn void SickTiM55xCommonAcquisitionThread::close_device()
52  * Close the device.
53  * Virtual method implemented by the actual connection driver.
54  *
55  * @fn void SickTiM55xCommonAcquisitionThread::flush_device()
56  * Flush the device.
57  * Read all current data on the channel and return on no data to read
58  * or timeout.
59  *
60  * @var std::string SickTiM55xCommonAcquisitionThread::cfg_name_
61  * Name of the particular configuration instance.
62  *
63  * @var std::string SickTiM55xCommonAcquisitionThread::cfg_prefix_
64  * Configuration path prefix for this configuration.
65  *
66  * @var std::string SickTiM55xCommonAcquisitionThread::dev_model_
67  * Device model type as string.
68  */
69 
70 /** Constructor.
71  * @param cfg_name short name of configuration group
72  * @param cfg_prefix configuration path prefix
73  */
75  std::string &cfg_prefix)
76 : LaserAcquisitionThread("SickTiM55xCommonAcquisitionThread"),
77  ConfigurationChangeHandler(cfg_prefix.c_str())
78 {
79  set_name("SickTiM55x(%s)", cfg_name.c_str());
80  pre_init_done_ = false;
81  cfg_name_ = cfg_name;
82  cfg_prefix_ = cfg_prefix;
83 }
84 
85 /** Destructor. */
87 {
88 }
89 
90 void
92 {
93  if (pre_init_done_)
94  return;
95  pre_init_done_ = true;
96 
97  if (dev_model_.empty()) {
98  throw Exception("LaserSick5xx: model has not yet been determined");
99  }
100 
101  if (dev_model_ == "TiM5xx") {
102  _distances_size = 360;
103  _echoes_size = 360;
104  expected_num_data_ = 271;
105  } else if (dev_model_ == "TiM571") {
106  _distances_size = 1080;
107  _echoes_size = 1080;
108  expected_num_data_ = 811;
109  } else {
110  throw Exception("LaserSick5xx: unknown model %s", dev_model_.c_str());
111  }
112 
115 
116  config->add_change_handler(this);
117 }
118 
119 /** Read common configuration parameters. */
120 void
122 {
123  cfg_time_offset_ = 0.;
124  try {
125  cfg_time_offset_ += config->get_float((cfg_prefix_ + "time_offset").c_str());
126  } catch (Exception &e) {
127  } // ignored, use default
128  logger->log_debug(name(), "Time offset: %f", cfg_time_offset_);
129 }
130 
131 /** Initialize device. */
132 void
134 {
135  open_device();
136 
137  // turn off data transfer, just in case...
138  try {
139  const char *req_scan_data = "\x02sEN LMDscandata 0\x03";
140  send_with_reply(req_scan_data);
141  } catch (Exception &e) {
142  } // ignore
143 
144  flush_device();
145 
146  std::string rep_dev_indent;
147  try {
148  const char *req_dev_indent = "\x02sRI0\x03\0";
149  send_with_reply(req_dev_indent, &rep_dev_indent);
150  } catch (Exception &e) {
151  close_device();
152  e.append("Failed to get device indent");
153  throw;
154  }
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(" "));
158  logger->log_debug(name(), "Ident: %s", rep_dev_indent.c_str());
159 
160  try {
161  const char *req_scan_data = "\x02sEN LMDscandata 1\x03";
162  send_with_reply(req_scan_data);
163  } catch (Exception &e) {
164  close_device();
165  e.append("Failed to start data streaming");
166  throw;
167  }
168 }
169 
170 /** Resynchronize to laser data.
171  * Stop data transfer, flush, restart.
172  */
173 void
175 {
176  // turn off data transfer
177  try {
178  const char *req_scan_data = "\x02sEN LMDscandata 0\x03";
179  send_with_reply(req_scan_data);
180  } catch (Exception &e) {
181  } // ignore
182 
183  flush_device();
184 
185  // turn on data transfer
186  try {
187  const char *req_scan_data = "\x02sEN LMDscandata 1\x03";
188  send_with_reply(req_scan_data);
189  } catch (Exception &e) {
190  } // ignore
191 }
192 
193 /** Parse incoming message from device.
194  * Based on https://www.mysick.com/saqqara/pdf.aspx?id=im0053129 and
195  * https://github.com/uos/sick_tim3xx.
196  * @param datagram data content
197  * @param datagram_length length in bytes of @p datagram
198  */
199 void
201  size_t datagram_length)
202 {
203  static const size_t HEADER_FIELDS = 33;
204 
205  std::string datagram_s((const char *)datagram, datagram_length);
206  std::vector<std::string> fields = str_split(datagram_s, ' ');
207 
208  size_t count = fields.size();
209 
210  // Validate header. Total number of tokens is highly unreliable as this may
211  // change when you change the scanning range or the device name using SOPAS ET
212  // tool. The header remains stable, however.
213  if (count < HEADER_FIELDS) {
214  throw Exception("Insufficient number of fields received");
215  }
216  if (fields[15] != "0") {
217  throw Exception("Invalid datagram format, ignoring scan");
218  }
219  if (fields[20] != "DIST1") {
220  throw Exception("Invalid datagram format (DIST1), ignoring scan");
221  }
222 
223  // More in depth checks: check data length and RSSI availability
224  // 25: Number of data (<= 10F)
225  unsigned short int number_of_data = 0;
226  sscanf(fields[25].c_str(), "%hx", &number_of_data);
227 
228  if (number_of_data != expected_num_data_) {
229  throw Exception("Invalid data length, got %u, expected %u", number_of_data, expected_num_data_);
230  }
231  if (count < HEADER_FIELDS + number_of_data) {
232  throw Exception("Invalid number of fields received, got %zu, expected %u+%u=%u",
233  count,
234  HEADER_FIELDS,
235  number_of_data,
236  HEADER_FIELDS + number_of_data);
237  }
238 
239  // Calculate offset of field that contains indicator of whether or not RSSI data is included
240  size_t rssi_idx = 26 + number_of_data;
241  int tmp;
242  sscanf(fields[rssi_idx].c_str(), "%d", &tmp);
243  bool rssi = tmp > 0;
244  unsigned short int number_of_rssi_data = 0;
245  if (rssi) {
246  sscanf(fields[rssi_idx + 6].c_str(), "%hx", &number_of_rssi_data);
247 
248  // Number of RSSI data should be equal to number of 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)",
251  number_of_data,
252  number_of_rssi_data);
253  }
254 
255  // Check if the total length is still appropriate.
256  // RSSI data size = number of RSSI readings + 6 fields describing the 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);
259  }
260 
261  if (fields[rssi_idx + 1] != "RSSI1") {
262  throw Exception("Field %zu of received data is not equal to RSSI1 (%s)",
263  rssi_idx + 1,
264  fields[rssi_idx + 1].c_str());
265  }
266  }
267 
268  // <STX> (\x02)
269  // 0: Type of command (SN)
270  // 1: Command (LMDscandata)
271  // 2: Firmware version number (1)
272  // 3: Device number (1)
273  // 4: Serial number (eg. B96518)
274  // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
275  // 7: Telegram counter (eg. 99)
276  // 8: Scan counter (eg. 9A)
277  // 9: Time since startup (eg. 13C8E59)
278  // 10: Time of transmission (eg. 13C9CBE)
279  // 11 + 12: Input status (0 0)
280  // 13 + 14: Output status (8 0)
281  // 15: Reserved Byte A (0)
282 
283  // 16: Scanning Frequency (5DC)
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);
287 
288  // 17: Measurement Frequency (36)
289  // this yields wrong results on some devices
290  //unsigned short measurement_freq = -1;
291  //sscanf(fields[17].c_str(), "%hx", &measurement_freq);
292  //float time_increment = 1.0 / (measurement_freq * 100.0);
293 
294  // 18: Number of encoders (0)
295  // 19: Number of 16 bit channels (1)
296  // 20: Measured data contents (DIST1)
297 
298  // 21: Scaling factor (3F800000)
299  // ignored for now (is always 1.0):
300  // unsigned int scaling_factor_int = -1;
301  // sscanf(fields[21], "%x", &scaling_factor_int);
302  // float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
303 
304  // 22: Scaling offset (00000000) -- always 0
305  // 23: Starting angle (FFF92230)
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;
310 
311  // 24: Angular step width (2710)
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);
316  //float angle_max = angle_min + (number_of_data - 1) * angle_increment;
317 
318  // 25: Number of data (<= 10F)
319  // This is already determined above in number_of_data
320 
321  // 26..26 + n - 1: Data_1 .. Data_n
322  _data_mutex->lock();
323  _timestamp->stamp();
324 
325  int start_idx = (int)roundf(rad2deg(angle_min) / angle_increment_deg);
326 
327  for (int j = 0; j < number_of_data; ++j) {
328  unsigned short range;
329  sscanf(fields[j + 26].c_str(), "%hx", &range);
330  int idx = (_distances_size + start_idx + j) % _distances_size;
331  _distances[idx] = range / 1000.0;
332  }
333 
334  if (rssi) {
335  // 26 + n: RSSI data included
336 
337  // 26 + n + 1 = RSSI Measured Data Contents (RSSI1)
338  // 26 + n + 2 = RSSI scaling factor (3F80000)
339  // 26 + n + 3 = RSSI Scaling offset (0000000)
340  // 26 + n + 4 = RSSI starting angle (equal to Range starting angle)
341  // 26 + n + 5 = RSSI angular step width (equal to Range angular step width)
342  // 26 + n + 6 = RSSI number of data (equal to Range number of data)
343  // 26 + n + 7 .. 26 + n + 7 + n - 1: RSSI_Data_1 .. RSSI_Data_n
344  // 26 + n + 7 + n .. 26 + n + 7 + n + 2 = unknown (but seems to be [0, 1, B] always)
345  // 26 + n + 7 + n + 2 .. count - 4 = device label
346  // count - 3 .. count - 1 = unknown (but seems to be 0 always)
347  // <ETX> (\x03)
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);
352  int idx = (_echoes_size + start_idx + j) % _echoes_size;
353  _echoes[idx] = intensity;
354  }
355  }
356 
357  _new_data = true;
358 
359  float time_increment = scan_time * angle_increment / (2.0 * M_PI);
360 
361  *_timestamp -= (double)number_of_data * time_increment;
362  *_timestamp += cfg_time_offset_;
363 
364  _data_mutex->unlock();
365 
366  // 26 + n: RSSI data included
367  // IF RSSI not included:
368  // 26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
369  // 26 + n + 4 .. count - 4 = device label
370  // count - 3 .. count - 1 = unknown (but seems to be 0 always)
371  // <ETX> (\x03)
372 }
373 
374 void
375 SickTiM55xCommonAcquisitionThread::config_value_changed(
377 {
378  MutexLocker lock(loop_mutex);
380 }
381 
382 void
383 SickTiM55xCommonAcquisitionThread::config_value_erased(const char *path)
384 {
385  MutexLocker lock(loop_mutex);
387 }
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.
Laser acqusition thread.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
Mutex locking helper.
Definition: mutex_locker.h:33
Interface for configuration change handling.
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().
Definition: thread.h:152
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
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.
Definition: thread.cpp:748
Base class for exceptions in Fawkes.
Definition: exception.h:35
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.
Definition: thread.h:100
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.
Definition: angle.h:46
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Iterator interface to iterate over config values.
Definition: config.h:71
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.
Definition: mutex.cpp:87
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Definition: config.cpp:603
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
std::string dev_model_
Device model type as string.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Interface for configuration handling.
Definition: config.h:64
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.
Definition: exception.cpp:333
Interface for logging.
Definition: logger.h:41