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 
46 using 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 
70 void
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 {
93  init_device();
94  } catch (...) {
95  libusb_exit(usb_ctx_);
96  throw;
97  }
98 
100 }
101 
102 void
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 
129 void
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 {
151  logger->log_warn(name(),
152  "Failed to read Sick TiM55x data (%d): %s",
153  usb_rv,
154  libusb_strerror((libusb_error)usb_rv));
155  }
156  reset_distances();
157  reset_echoes();
158  return;
159  } else {
160  recv_buf[actual_length] = 0;
161  lock.unlock();
162 
163  reset_distances();
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 
188 void
189 SickTiM55xUSBAcquisitionThread::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) {
214  logger->log_warn(name(),
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) {
233  logger->log_warn(name(),
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 {
246  logger->log_info(name(),
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 
283 void
284 SickTiM55xUSBAcquisitionThread::close_device()
285 {
286  libusb_release_interface(usb_device_handle_, 0);
287  libusb_close(usb_device_handle_);
288  usb_device_handle_ = NULL;
289 }
290 
291 void
292 SickTiM55xUSBAcquisitionThread::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 
313 void
314 SickTiM55xUSBAcquisitionThread::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 }
SickTiM55xCommonAcquisitionThread::parse_datagram
void parse_datagram(const unsigned char *datagram, size_t datagram_length)
Parse incoming message from device.
Definition: sick_tim55x_common_aqt.cpp:200
fawkes::Mutex
Mutex mutual exclusion lock.
Definition: mutex.h:33
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::MutexLocker
Mutex locking helper.
Definition: mutex_locker.h:34
LaserAcquisitionThread::_echoes
float * _echoes
Allocate a float array and copy your echo values here.
Definition: acquisition_thread.h:79
fawkes::Thread::yield
void yield()
Yield the processor to another thread or process.
Definition: thread.cpp:883
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
LaserAcquisitionThread::reset_distances
void reset_distances()
Reset all distance values to NaN.
Definition: acquisition_thread.cpp:202
SickTiM55xUSBAcquisitionThread::SickTiM55xUSBAcquisitionThread
SickTiM55xUSBAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Definition: sick_tim55x_usb_aqt.cpp:62
SickTiM55xUSBAcquisitionThread::loop
virtual void loop()
Code to execute in the thread.
Definition: sick_tim55x_usb_aqt.cpp:130
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
SickTiM55xCommonAcquisitionThread::read_common_config
void read_common_config()
Read common configuration parameters.
Definition: sick_tim55x_common_aqt.cpp:121
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
SickTiM55xCommonAcquisitionThread::pre_init
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
Definition: sick_tim55x_common_aqt.cpp:91
SickTiM55xUSBAcquisitionThread::finalize
virtual void finalize()
Finalize the thread.
Definition: sick_tim55x_usb_aqt.cpp:103
fawkes
Fawkes library namespace.
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
LaserAcquisitionThread::_distances
float * _distances
Allocate a float array and copy your distance values measured in meters here.
Definition: acquisition_thread.h:78
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
LaserAcquisitionThread::reset_echoes
void reset_echoes()
Reset all distance values to NaN.
Definition: acquisition_thread.cpp:217
SickTiM55xCommonAcquisitionThread::cfg_prefix_
std::string cfg_prefix_
Configuration path prefix for this configuration.
Definition: sick_tim55x_common_aqt.h:71
SickTiM55xCommonAcquisitionThread::init_device
void init_device()
Initialize device.
Definition: sick_tim55x_common_aqt.cpp:133
SickTiM55xUSBAcquisitionThread::init
virtual void init()
Initialize the thread.
Definition: sick_tim55x_usb_aqt.cpp:71
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
SickTiM55xCommonAcquisitionThread
Laser acqusition thread for Sick TiM55x laser range finders.
Definition: sick_tim55x_common_aqt.h:39
fawkes::MutexLocker::unlock
void unlock()
Unlock the mutex.
Definition: mutex_locker.cpp:153
fawkes::Thread::set_name
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
SickTiM55xCommonAcquisitionThread::resync
void resync()
Resynchronize to laser data.
Definition: sick_tim55x_common_aqt.cpp:174
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36