23 #include "sick_tim55x_usb_aqt.h" 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> 36 #ifndef LIBUSB_API_VERSION 37 # define libusb_error_name(error) "" 38 # define LIBUSB_LOG_LEVEL_ERROR 1 41 #if LIBUSBX_API_VERSION < 0x01000102 43 # define libusb_strerror libusb_error_name 48 #define USB_VENDOR 0x19A2 49 #define USB_PRODUCT 0x5001 50 #define USB_TIMEOUT 500 63 std::string &cfg_prefix)
66 set_name(
"SickTiM55xUSB(%s)", cfg_name.c_str());
67 usb_device_handle_ = NULL;
81 if ((usb_rv = libusb_init(&usb_ctx_)) != 0) {
82 throw Exception(
"Failed to init libusb: %s", libusb_strerror((libusb_error)usb_rv));
84 #if defined(LIBUSB_API_VERSION) && (LIBUSB_API_VERSION >= 0x01000106) 85 libusb_set_option(usb_ctx_, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_ERROR);
87 libusb_set_debug(usb_ctx_, LIBUSB_LOG_LEVEL_ERROR);
90 usb_mutex_ =
new Mutex();
95 libusb_exit(usb_ctx_);
105 if (usb_device_handle_) {
107 const char *req_scan_data =
"\x02sEN LMDscandata 0\x03";
108 send_with_reply(req_scan_data);
113 if ((usb_rv = libusb_release_interface(usb_device_handle_, 0)) != 0) {
116 libusb_close(usb_device_handle_);
118 libusb_exit(usb_ctx_);
132 int actual_length = 0;
134 if (usb_device_handle_) {
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),
146 if (usb_rv == LIBUSB_ERROR_NO_DEVICE) {
148 libusb_close(usb_device_handle_);
149 usb_device_handle_ = NULL;
152 "Failed to read Sick TiM55x data (%d): %s",
154 libusb_strerror((libusb_error)usb_rv));
160 recv_buf[actual_length] = 0;
180 usleep(USB_TIMEOUT * 1000);
189 SickTiM55xUSBAcquisitionThread::open_device()
191 if (usb_device_handle_)
194 libusb_device **devices;
195 ssize_t num_devs = libusb_get_device_list(usb_ctx_, &devices);
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);
203 if (desc.idVendor == USB_VENDOR && desc.idProduct == USB_PRODUCT) {
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.");
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));
220 if (cfg_serial_ !=
"") {
221 if (desc.iSerialNumber == 0) {
226 unsigned char serial_desc[32];
227 usb_rv = libusb_get_string_descriptor_ascii(usb_device_handle_,
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;
241 std::string serial_desc_s((
const char *)serial_desc, usb_rv);
243 if (cfg_serial_ == serial_desc_s) {
247 "Ignoring Sick TiM55x with non-matching serial %s" 249 serial_desc_s.c_str(),
250 cfg_serial_.c_str());
251 libusb_close(usb_device_handle_);
252 usb_device_handle_ = NULL;
258 libusb_free_device_list(devices, 1);
260 if (usb_device_handle_ != NULL) {
262 if (libusb_kernel_driver_active(usb_device_handle_, 0) == 1) {
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));
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));
279 throw Exception(
"No matching device found");
284 SickTiM55xUSBAcquisitionThread::close_device()
286 libusb_release_interface(usb_device_handle_, 0);
287 libusb_close(usb_device_handle_);
288 usb_device_handle_ = NULL;
292 SickTiM55xUSBAcquisitionThread::flush_device()
294 if (usb_device_handle_) {
297 int actual_length = 0;
298 size_t recv_buf_size = 32 * 1024;
299 unsigned char recv_buf[recv_buf_size];
301 usb_rv = libusb_bulk_transfer(usb_device_handle_,
302 (1 | LIBUSB_ENDPOINT_IN),
309 }
while (usb_rv == 0 && actual_length > 0);
314 SickTiM55xUSBAcquisitionThread::send_with_reply(
const char *request, std::string *reply)
319 int actual_length = 0;
320 int request_length = strlen(request);
322 usb_rv = libusb_bulk_transfer(usb_device_handle_,
323 (2 | LIBUSB_ENDPOINT_OUT),
324 (
unsigned char *)request,
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));
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);
337 throw Exception(
"Sick TiM55x: failed to read reply (%s)",
338 libusb_strerror((libusb_error)usb_rv));
342 *reply = std::string((
const char *)tmpbuf, actual_length);
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.
virtual void finalize()
Finalize the thread.
void init_device()
Initialize device.
Logger * logger
This is the Logger member used to access the logger.
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.
Base class for exceptions in Fawkes.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
const char * name() const
Get name of thread.
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.
void read_common_config()
Read common configuration parameters.
Mutex mutual exclusion lock.
Configuration * config
This is the Configuration member used to access the configuration.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
float * _echoes
Allocate a float array and copy your echo values here.
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.