Fawkes API Fawkes Development Version
sick_tim55x_usb_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_usb_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 <libusb.h>
34#include <unistd.h>
35
36#ifndef LIBUSB_API_VERSION
37# define libusb_error_name(error) ""
38# define LIBUSB_LOG_LEVEL_ERROR 1
39#endif
40
41#if LIBUSBX_API_VERSION < 0x01000102
42// libusb before 1.0.16 does not have libusb_strerror
43# define libusb_strerror libusb_error_name
44#endif
45
46using namespace fawkes;
47
48#define USB_VENDOR 0x19A2
49#define USB_PRODUCT 0x5001
50#define USB_TIMEOUT 500
51
52/** @class SickTiM55xUSBAcquisitionThread "sick_tim55x_usb_aqt.h"
53 * Laser acqusition thread for Sick TiM55x laser range finders.
54 * This thread fetches the data from the laser.
55 * @author Tim Niemueller
56 */
57
58/** Constructor.
59 * @param cfg_name short name of configuration group
60 * @param cfg_prefix configuration path prefix
61 */
63 std::string &cfg_prefix)
64: SickTiM55xCommonAcquisitionThread(cfg_name, cfg_prefix)
65{
66 set_name("SickTiM55xUSB(%s)", cfg_name.c_str());
67 usb_device_handle_ = NULL;
68}
69
70void
72{
74
75 try {
76 cfg_serial_ = config->get_string((cfg_prefix_ + "serial").c_str());
77 } catch (Exception &e) {
78 } // ignore, if there is only one take that
79
80 int usb_rv = 0;
81 if ((usb_rv = libusb_init(&usb_ctx_)) != 0) {
82 throw Exception("Failed to init libusb: %s", libusb_strerror((libusb_error)usb_rv));
83 }
84#if defined(LIBUSB_API_VERSION) && (LIBUSB_API_VERSION >= 0x01000106)
85 libusb_set_option(usb_ctx_, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_ERROR);
86#else
87 libusb_set_debug(usb_ctx_, LIBUSB_LOG_LEVEL_ERROR);
88#endif
89
90 usb_mutex_ = new Mutex();
91
92 try {
94 } catch (...) {
95 libusb_exit(usb_ctx_);
96 throw;
97 }
98
100}
101
102void
104{
105 if (usb_device_handle_) {
106 try {
107 const char *req_scan_data = "\x02sEN LMDscandata 0\x03";
108 send_with_reply(req_scan_data);
109 } catch (Exception &e) {
110 } // ignore
111
112 int usb_rv = 0;
113 if ((usb_rv = libusb_release_interface(usb_device_handle_, 0)) != 0) {
114 logger->log_warn(name(), "Sick TiM55x: failed to release device");
115 }
116 libusb_close(usb_device_handle_);
117 }
118 libusb_exit(usb_ctx_);
119
120 free(_distances);
121 _distances = NULL;
122
123 free(_echoes);
124 _echoes = NULL;
125
126 delete usb_mutex_;
127}
128
129void
131{
132 int actual_length = 0;
133
134 if (usb_device_handle_) {
135 MutexLocker lock(usb_mutex_);
136 int usb_rv = 0;
137 size_t recv_buf_size = 32 * 1024;
138 unsigned char recv_buf[recv_buf_size];
139 usb_rv = libusb_bulk_transfer(usb_device_handle_,
140 (1 | LIBUSB_ENDPOINT_IN),
141 recv_buf,
142 recv_buf_size - 1,
143 &actual_length,
144 USB_TIMEOUT);
145 if (usb_rv != 0) {
146 if (usb_rv == LIBUSB_ERROR_NO_DEVICE) {
147 logger->log_error(name(), "Device disconnected, will try to reconnect");
148 libusb_close(usb_device_handle_);
149 usb_device_handle_ = NULL;
150 } else {
152 "Failed to read Sick TiM55x data (%d): %s",
153 usb_rv,
154 libusb_strerror((libusb_error)usb_rv));
155 }
157 reset_echoes();
158 return;
159 } else {
160 recv_buf[actual_length] = 0;
161 lock.unlock();
162
164 reset_echoes();
165
166 try {
167 parse_datagram(recv_buf, actual_length);
168 } catch (Exception &e) {
169 logger->log_warn(name(), "Failed to parse datagram, resyncing, exception follows");
170 logger->log_warn(name(), e);
171 resync();
172 }
173 }
174 } else {
175 try {
176 init_device();
177 logger->log_warn(name(), "Reconnected to device");
178 } catch (Exception &e) {
179 // ignore, keep trying
180 usleep(USB_TIMEOUT * 1000);
181 return;
182 }
183 }
184
185 yield();
186}
187
188void
189SickTiM55xUSBAcquisitionThread::open_device()
190{
191 if (usb_device_handle_)
192 return;
193
194 libusb_device **devices;
195 ssize_t num_devs = libusb_get_device_list(usb_ctx_, &devices);
196
197 for (ssize_t i = 0; i < num_devs; ++i) {
198 libusb_device_descriptor desc;
199 int usb_rv = libusb_get_device_descriptor(devices[i], &desc);
200 if (usb_rv != 0)
201 continue;
202
203 if (desc.idVendor == USB_VENDOR && desc.idProduct == USB_PRODUCT) {
204 // found a device
205
206 if (usb_device_handle_ != NULL) {
207 libusb_close(usb_device_handle_);
208 usb_device_handle_ = NULL;
209 libusb_free_device_list(devices, 1);
210 throw Exception("Two devices found, specify serial of device to use.");
211 }
212
213 if ((usb_rv = libusb_open(devices[i], &usb_device_handle_)) != 0) {
215 "Failed to open Sick TiM55x: %s",
216 libusb_strerror((libusb_error)usb_rv));
217 continue;
218 }
219
220 if (cfg_serial_ != "") {
221 if (desc.iSerialNumber == 0) {
222 continue;
223 }
224
225 // read serial from device
226 unsigned char serial_desc[32];
227 usb_rv = libusb_get_string_descriptor_ascii(usb_device_handle_,
228 desc.iSerialNumber,
229 serial_desc,
230 32);
231
232 if (usb_rv <= 0) {
234 "Failed to read serial from Sick TiM55x: %s",
235 libusb_strerror((libusb_error)usb_rv));
236 libusb_close(usb_device_handle_);
237 usb_device_handle_ = NULL;
238 continue;
239 }
240
241 std::string serial_desc_s((const char *)serial_desc, usb_rv);
242
243 if (cfg_serial_ == serial_desc_s) {
244 break;
245 } else {
247 "Ignoring Sick TiM55x with non-matching serial %s"
248 " (looking for %s)",
249 serial_desc_s.c_str(),
250 cfg_serial_.c_str());
251 libusb_close(usb_device_handle_);
252 usb_device_handle_ = NULL;
253 }
254 }
255 }
256 }
257
258 libusb_free_device_list(devices, 1);
259
260 if (usb_device_handle_ != NULL) {
261 int usb_rv;
262 if (libusb_kernel_driver_active(usb_device_handle_, 0) == 1) {
263 logger->log_info(name(), "Kernel driver active, disabling");
264 if ((usb_rv = libusb_detach_kernel_driver(usb_device_handle_, 0)) != 0) {
265 libusb_close(usb_device_handle_);
266 usb_device_handle_ = NULL;
267 throw Exception("Sick TiM55x: failed to detach kernel driver (%s)",
268 libusb_strerror((libusb_error)usb_rv));
269 }
270 }
271
272 if ((usb_rv = libusb_claim_interface(usb_device_handle_, 0)) != 0) {
273 libusb_close(usb_device_handle_);
274 usb_device_handle_ = NULL;
275 throw Exception("Sick TiM55x: failed to claim device (%s)",
276 libusb_strerror((libusb_error)usb_rv));
277 }
278 } else {
279 throw Exception("No matching device found");
280 }
281}
282
283void
284SickTiM55xUSBAcquisitionThread::close_device()
285{
286 libusb_release_interface(usb_device_handle_, 0);
287 libusb_close(usb_device_handle_);
288 usb_device_handle_ = NULL;
289}
290
291void
292SickTiM55xUSBAcquisitionThread::flush_device()
293{
294 if (usb_device_handle_) {
295 MutexLocker lock(usb_mutex_);
296 int usb_rv = 0;
297 int actual_length = 0;
298 size_t recv_buf_size = 32 * 1024;
299 unsigned char recv_buf[recv_buf_size];
300 do {
301 usb_rv = libusb_bulk_transfer(usb_device_handle_,
302 (1 | LIBUSB_ENDPOINT_IN),
303 recv_buf,
304 recv_buf_size - 1,
305 &actual_length,
306 USB_TIMEOUT);
307
308 // we don't care, we just want to get rid of data
309 } while (usb_rv == 0 && actual_length > 0);
310 }
311}
312
313void
314SickTiM55xUSBAcquisitionThread::send_with_reply(const char *request, std::string *reply)
315{
316 MutexLocker lock(usb_mutex_);
317
318 int usb_rv = 0;
319 int actual_length = 0;
320 int request_length = strlen(request);
321
322 usb_rv = libusb_bulk_transfer(usb_device_handle_,
323 (2 | LIBUSB_ENDPOINT_OUT),
324 (unsigned char *)request,
325 request_length,
326 &actual_length,
327 USB_TIMEOUT);
328 if (usb_rv != 0 || actual_length != request_length) {
329 throw Exception("Sick TiM55x: failed to send request (%s)",
330 libusb_strerror((libusb_error)usb_rv));
331 }
332
333 unsigned char tmpbuf[32 * 1024];
334 usb_rv = libusb_bulk_transfer(
335 usb_device_handle_, (1 | LIBUSB_ENDPOINT_IN), tmpbuf, 32 * 1024, &actual_length, USB_TIMEOUT);
336 if (usb_rv != 0) {
337 throw Exception("Sick TiM55x: failed to read reply (%s)",
338 libusb_strerror((libusb_error)usb_rv));
339 }
340
341 if (reply) {
342 *reply = std::string((const char *)tmpbuf, actual_length);
343 }
344}
float * _distances
Allocate a float array and copy your distance values measured in meters here.
void reset_distances()
Reset all distance values to NaN.
float * _echoes
Allocate a float array and copy your echo values here.
void reset_echoes()
Reset all distance values to NaN.
Laser acqusition thread for Sick TiM55x laser range finders.
void read_common_config()
Read common configuration parameters.
void resync()
Resynchronize to laser data.
std::string cfg_prefix_
Configuration path prefix for this configuration.
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.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
SickTiM55xUSBAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void init()
Initialize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational 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 unlock()
Unlock the mutex.
Mutex mutual exclusion lock.
Definition: mutex.h:33
const char * name() const
Get name of thread.
Definition: thread.h:100
void yield()
Yield the processor to another thread or process.
Definition: thread.cpp:883
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Fawkes library namespace.