23 #include "sick_tim55x_usb_aqt.h" 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> 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 65 std::string &cfg_prefix)
68 set_name(
"SickTiM55xUSB(%s)", cfg_name.c_str());
69 usb_device_handle_ = NULL;
82 if ((usb_rv = libusb_init(&usb_ctx_)) != 0) {
83 throw Exception(
"Failed to init libusb: %s", libusb_strerror((libusb_error)usb_rv));
85 libusb_set_debug(usb_ctx_, LIBUSB_LOG_LEVEL_ERROR);
87 usb_mutex_ =
new Mutex();
92 libusb_exit(usb_ctx_);
103 if (usb_device_handle_) {
105 const char *req_scan_data =
"\x02sEN LMDscandata 0\x03";
106 send_with_reply(req_scan_data);
110 if ((usb_rv = libusb_release_interface(usb_device_handle_, 0)) != 0) {
113 libusb_close(usb_device_handle_);
115 libusb_exit(usb_ctx_);
127 int actual_length = 0;
128 size_t recv_buf_size = 32*1024;
129 unsigned char recv_buf[recv_buf_size];
131 if (usb_device_handle_) {
134 usb_rv = libusb_bulk_transfer(usb_device_handle_, (1 | LIBUSB_ENDPOINT_IN),
135 recv_buf, recv_buf_size - 1, &actual_length,
138 if (usb_rv == LIBUSB_ERROR_NO_DEVICE) {
140 libusb_close(usb_device_handle_);
141 usb_device_handle_ = NULL;
144 usb_rv, libusb_strerror((libusb_error)usb_rv));
150 recv_buf[actual_length] = 0;
170 usleep(USB_TIMEOUT * 1000);
180 SickTiM55xUSBAcquisitionThread::open_device()
182 if (usb_device_handle_)
return;
184 libusb_device **devices;
185 ssize_t num_devs = libusb_get_device_list(usb_ctx_, &devices);
187 for (ssize_t i = 0; i < num_devs; ++i)
189 libusb_device_descriptor desc;
190 int usb_rv = libusb_get_device_descriptor(devices[i], &desc);
191 if (usb_rv != 0)
continue;
193 if (desc.idVendor == USB_VENDOR && desc.idProduct == USB_PRODUCT) {
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.");
203 if ((usb_rv = libusb_open(devices[i], &usb_device_handle_)) != 0) {
205 libusb_strerror((libusb_error) usb_rv));
209 if (cfg_serial_ !=
"") {
210 if (desc.iSerialNumber == 0) {
215 unsigned char serial_desc[32];
216 usb_rv = libusb_get_string_descriptor_ascii(usb_device_handle_, desc.iSerialNumber,
221 libusb_strerror((libusb_error) usb_rv));
222 libusb_close(usb_device_handle_);
223 usb_device_handle_ = NULL;
227 std::string serial_desc_s((
const char *)serial_desc, usb_rv);
229 if (cfg_serial_ == serial_desc_s) {
234 serial_desc_s.c_str(), cfg_serial_.c_str());
235 libusb_close(usb_device_handle_);
236 usb_device_handle_ = NULL;
242 libusb_free_device_list(devices, 1);
244 if (usb_device_handle_ != NULL) {
246 if (libusb_kernel_driver_active(usb_device_handle_, 0) == 1) {
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));
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));
263 throw Exception(
"No matching device found");
269 SickTiM55xUSBAcquisitionThread::close_device()
271 libusb_release_interface(usb_device_handle_, 0);
272 libusb_close(usb_device_handle_);
273 usb_device_handle_ = NULL;
277 SickTiM55xUSBAcquisitionThread::flush_device()
279 if (usb_device_handle_) {
282 int actual_length = 0;
283 size_t recv_buf_size = 32*1024;
284 unsigned char recv_buf[recv_buf_size];
286 usb_rv = libusb_bulk_transfer(usb_device_handle_, (1 | LIBUSB_ENDPOINT_IN),
287 recv_buf, recv_buf_size - 1, &actual_length,
291 }
while (usb_rv == 0 && actual_length > 0);
297 SickTiM55xUSBAcquisitionThread::send_with_reply(
const char *request,
303 int actual_length = 0;
304 int request_length = strlen(request);
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));
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);
318 throw Exception(
"Sick TiM55x: failed to read reply (%s)",
319 libusb_strerror((libusb_error)usb_rv));
323 *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.
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.