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
35using 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
90void
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
117}
118
119/** Read common configuration parameters. */
120void
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. */
132void
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 */
173void
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 */
199void
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
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
374void
375SickTiM55xCommonAcquisitionThread::config_value_changed(
377{
380}
381
382void
383SickTiM55xCommonAcquisitionThread::config_value_erased(const char *path)
384{
387}
Laser acqusition thread.
void alloc_echoes(unsigned int num_echoes)
Allocate echoes array.
unsigned int _echoes_size
Assign this the size of the _echoes array.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
bool _new_data
Set to true in your loop if new data is available.
void alloc_distances(unsigned int num_distances)
Allocate distances array.
unsigned int _distances_size
Assign this the size of the _distances array.
float * _echoes
Allocate a float array and copy your echo values here.
fawkes::Time * _timestamp
Time when the most recent data was received.
SickTiM55xCommonAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
void read_common_config()
Read common configuration parameters.
std::string cfg_name_
Name of the particular configuration instance.
virtual void close_device()=0
Close the device.
virtual void open_device()=0
Open the device.
void resync()
Resynchronize to laser data.
std::string cfg_prefix_
Configuration path prefix for this configuration.
virtual void flush_device()=0
Flush the device.
virtual void send_with_reply(const char *request, std::string *reply=NULL)=0
Send a request and expect a reply.
virtual ~SickTiM55xCommonAcquisitionThread()
Destructor.
std::string dev_model_
Device model type as string.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
void parse_datagram(const unsigned char *datagram, size_t datagram_length)
Parse incoming message from device.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Interface for configuration change handling.
Iterator interface to iterate over config values.
Definition: config.h:75
Interface for configuration handling.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Definition: config.cpp:603
Base class for exceptions in Fawkes.
Definition: exception.h:36
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Fawkes library namespace.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46