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/misc/string_split.h>
28 #include <utils/math/angle.h>
29 
30 #include <cstdlib>
31 #include <cstdio>
32 #include <cstring>
33 #include <unistd.h>
34 
35 using namespace fawkes;
36 
37 
38 /** @class SickTiM55xCommonAcquisitionThread "sick_tim55x_common_aqt.h"
39  * Laser acqusition thread for Sick TiM55x laser range finders.
40  * This thread fetches the data from the laser.
41  * @author Tim Niemueller
42  *
43  * @fn void SickTiM55xCommonAcquisitionThread::send_with_reply(const char *request, std::string *reply = NULL)
44  * Send a request and expect a reply.
45  * @param request request to send
46  * @param reply upon returns contains the received reply, maybe NULL to ignore the reply
47  *
48  * @fn void SickTiM55xCommonAcquisitionThread::open_device()
49  * Open the device.
50  * Virtual method implemented by the actual connection driver.
51  *
52  * @fn void SickTiM55xCommonAcquisitionThread::close_device()
53  * Close the device.
54  * Virtual method implemented by the actual connection driver.
55  *
56  * @fn void SickTiM55xCommonAcquisitionThread::flush_device()
57  * Flush the device.
58  * Read all current data on the channel and return on no data to read
59  * or timeout.
60  *
61  * @var std::string SickTiM55xCommonAcquisitionThread::cfg_name_
62  * Name of the particular configuration instance.
63  *
64  * @var std::string SickTiM55xCommonAcquisitionThread::cfg_prefix_
65  * Configuration path prefix for this configuration.
66  *
67  * @var std::string SickTiM55xCommonAcquisitionThread::dev_model_
68  * Device model type as string.
69  */
70 
71 
72 /** Constructor.
73  * @param cfg_name short name of configuration group
74  * @param cfg_prefix configuration path prefix
75  */
77  std::string &cfg_prefix)
78  : LaserAcquisitionThread("SickTiM55xCommonAcquisitionThread")
79 {
80  set_name("SickTiM55x(%s)", cfg_name.c_str());
81  pre_init_done_ = false;
82  cfg_name_ = cfg_name;
83  cfg_prefix_ = cfg_prefix;
84 }
85 
86 
87 /** Destructor. */
89 {
90 }
91 
92 void
95 {
96  if (pre_init_done_) return;
97  pre_init_done_ = true;
98 
99  if (dev_model_.empty()) {
100  throw Exception("LaserSick5xx: model has not yet been determined");
101  }
102 
103  if (dev_model_ == "TiM5xx") {
104  _distances_size = 360;
105  expected_num_data_ = 271;
106  } else if (dev_model_ == "TiM571") {
107  _distances_size = 1080;
108  expected_num_data_ = 811;
109  } else {
110  throw Exception("LaserSick5xx: unknown model %s", dev_model_.c_str());
111  }
112 
114 }
115 
116 /** Read common configuration parameters. */
117 void
119 {
120  cfg_time_offset_ = 0.;
121  try {
122  cfg_time_offset_ += config->get_float((cfg_prefix_ + "time_offset").c_str());
123  } catch (Exception &e) {} // ignored, use default
124 }
125 
126 
127 /** Initialize device. */
128 void
130 {
131  open_device();
132 
133  // turn off data transfer, just in case...
134  try {
135  const char *req_scan_data = "\x02sEN LMDscandata 0\x03";
136  send_with_reply(req_scan_data);
137  } catch (Exception &e) {} // ignore
138 
139  flush_device();
140 
141  std::string rep_dev_indent;
142  try {
143  const char *req_dev_indent = "\x02sRI0\x03\0";
144  send_with_reply(req_dev_indent, &rep_dev_indent);
145  } catch (Exception &e) {
146  close_device();
147  e.append("Failed to get device indent");
148  throw;
149  }
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(" "));
153  logger->log_debug(name(), "Ident: %s", rep_dev_indent.c_str());
154 
155  try {
156  const char *req_scan_data = "\x02sEN LMDscandata 1\x03";
157  send_with_reply(req_scan_data);
158  } catch (Exception &e) {
159  close_device();
160  e.append("Failed to start data streaming");
161  throw;
162  }
163 }
164 
165 
166 /** Resynchronize to laser data.
167  * Stop data transfer, flush, restart.
168  */
169 void
171 {
172  // turn off data transfer
173  try {
174  const char *req_scan_data = "\x02sEN LMDscandata 0\x03";
175  send_with_reply(req_scan_data);
176  } catch (Exception &e) {} // ignore
177 
178  flush_device();
179 
180  // turn on data transfer
181  try {
182  const char *req_scan_data = "\x02sEN LMDscandata 1\x03";
183  send_with_reply(req_scan_data);
184  } catch (Exception &e) {} // ignore
185 
186 }
187 
188 /** Parse incoming message from device.
189  * Based on https://www.mysick.com/saqqara/pdf.aspx?id=im0053129 and
190  * https://github.com/uos/sick_tim3xx.
191  * @param datagram data content
192  * @param datagram_length length in bytes of @p datagram
193  */
194 void
196  size_t datagram_length)
197 {
198  static const size_t HEADER_FIELDS = 33;
199 
200  std::string datagram_s((const char *)datagram, datagram_length);
201  std::vector<std::string> fields = str_split(datagram_s, ' ');
202 
203  size_t count = fields.size();
204 
205  // Validate header. Total number of tokens is highly unreliable as this may
206  // change when you change the scanning range or the device name using SOPAS ET
207  // tool. The header remains stable, however.
208  if (count < HEADER_FIELDS) {
209  throw Exception("Insufficient number of fields received");
210  }
211  if (fields[15] != "0") {
212  throw Exception("Invalid datagram format, ignoring scan");
213  }
214  if (fields[20] != "DIST1") {
215  throw Exception("Invalid datagram format (DIST1), ignoring scan");
216  }
217 
218  // More in depth checks: check data length and RSSI availability
219  // 25: Number of data (<= 10F)
220  unsigned short int number_of_data = 0;
221  sscanf(fields[25].c_str(), "%hx", &number_of_data);
222 
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_);
226  }
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);
230  }
231 
232  // Calculate offset of field that contains indicator of whether or not RSSI data is included
233  size_t rssi_idx = 26 + number_of_data;
234  int tmp;
235  sscanf(fields[rssi_idx].c_str(), "%d", &tmp);
236  bool rssi = tmp > 0;
237  unsigned short int number_of_rssi_data = 0;
238  if (rssi) {
239  sscanf(fields[rssi_idx + 6].c_str(), "%hx", &number_of_rssi_data);
240 
241  // Number of RSSI data should be equal to number of 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);
245  }
246 
247  // Check if the total length is still appropriate.
248  // RSSI data size = number of RSSI readings + 6 fields describing the 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);
252  }
253 
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());
257  }
258  }
259 
260  // <STX> (\x02)
261  // 0: Type of command (SN)
262  // 1: Command (LMDscandata)
263  // 2: Firmware version number (1)
264  // 3: Device number (1)
265  // 4: Serial number (eg. B96518)
266  // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
267  // 7: Telegram counter (eg. 99)
268  // 8: Scan counter (eg. 9A)
269  // 9: Time since startup (eg. 13C8E59)
270  // 10: Time of transmission (eg. 13C9CBE)
271  // 11 + 12: Input status (0 0)
272  // 13 + 14: Output status (8 0)
273  // 15: Reserved Byte A (0)
274 
275  // 16: Scanning Frequency (5DC)
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);
279 
280  // 17: Measurement Frequency (36)
281  // this yields wrong results on some devices
282  //unsigned short measurement_freq = -1;
283  //sscanf(fields[17].c_str(), "%hx", &measurement_freq);
284  //float time_increment = 1.0 / (measurement_freq * 100.0);
285 
286  // 18: Number of encoders (0)
287  // 19: Number of 16 bit channels (1)
288  // 20: Measured data contents (DIST1)
289 
290  // 21: Scaling factor (3F800000)
291  // ignored for now (is always 1.0):
292  // unsigned int scaling_factor_int = -1;
293  // sscanf(fields[21], "%x", &scaling_factor_int);
294  // float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
295 
296  // 22: Scaling offset (00000000) -- always 0
297  // 23: Starting angle (FFF92230)
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;
301 
302  // 24: Angular step width (2710)
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);
307  //float angle_max = angle_min + (number_of_data - 1) * angle_increment;
308 
309  // 25: Number of data (<= 10F)
310  // This is already determined above in number_of_data
311 
312  // 26..26 + n - 1: Data_1 .. Data_n
313  _data_mutex->lock();
314  _timestamp->stamp();
315 
316  int start_idx = (int)roundf(rad2deg(angle_min) / angle_increment_deg);
317 
318  for (int j = 0; j < number_of_data; ++j) {
319  unsigned short range;
320  sscanf(fields[j + 26].c_str(), "%hx", &range);
321  int idx = (_distances_size + start_idx + j) % _distances_size;
322  _distances[idx] = range / 1000.0;
323  }
324 
325  if (rssi) {
326  // 26 + n: RSSI data included
327 
328  // 26 + n + 1 = RSSI Measured Data Contents (RSSI1)
329  // 26 + n + 2 = RSSI scaling factor (3F80000)
330  // 26 + n + 3 = RSSI Scaling offset (0000000)
331  // 26 + n + 4 = RSSI starting angle (equal to Range starting angle)
332  // 26 + n + 5 = RSSI angular step width (equal to Range angular step width)
333  // 26 + n + 6 = RSSI number of data (equal to Range number of data)
334  // 26 + n + 7 .. 26 + n + 7 + n - 1: RSSI_Data_1 .. RSSI_Data_n
335  // 26 + n + 7 + n .. 26 + n + 7 + n + 2 = unknown (but seems to be [0, 1, B] always)
336  // 26 + n + 7 + n + 2 .. count - 4 = device label
337  // count - 3 .. count - 1 = unknown (but seems to be 0 always)
338  // <ETX> (\x03)
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);
343  int idx = (_echoes_size + start_idx + j) % _echoes_size;
344  _echoes[idx] = intensity;
345  }
346  }
347 
348  _new_data = true;
349 
350  float time_increment = scan_time * angle_increment / (2.0 * M_PI);
351 
352  *_timestamp -= number_of_data * time_increment;
353  *_timestamp += cfg_time_offset_;
354 
355  _data_mutex->unlock();
356 
357  // 26 + n: RSSI data included
358  // IF RSSI not included:
359  // 26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
360  // 26 + n + 4 .. count - 4 = device label
361  // count - 3 .. count - 1 = unknown (but seems to be 0 always)
362  // <ETX> (\x03)
363 }
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:135
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.
Definition: logging.h:44
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:761
Base class for exceptions in Fawkes.
Definition: exception.h:36
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:95
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:48
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.
Definition: mutex.cpp:89
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
std::string dev_model_
Device model type as string.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
Interface for configuration handling.
Definition: config.h:67
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:341
Interface for logging.
Definition: logger.h:34