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