45 ctx_(NULL), numberOfDevices_(0), devices_(NULL), device_handle_(NULL), device_number_(device_number)
65 printf(
"LIBUSB - Cannot Release Interface!\n");
67 printf(
"LIBUSB - Released Interface.\n");
91 libusb_device ***list)
93 libusb_device **resultDevices = NULL;
94 ssize_t numberOfResultDevices = 0;
95 libusb_device **devices;
100 ssize_t numberOfDevices = libusb_get_device_list(ctx, &devices);
105 for (ssize_t i = 0; i < numberOfDevices; i++)
107 struct libusb_device_descriptor desc;
108 int result = libusb_get_device_descriptor(devices[i], &desc);
111 ROS_ERROR(
"LIBUSB - Failed to get device descriptor");
112 diagnostics_.
broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
"LIBUSB - Failed to get device descriptor.");
116 if (desc.idVendor == vendorID && desc.idProduct == 0x5001)
121 resultDevices = (libusb_device **)realloc(resultDevices,
sizeof(libusb_device *) * (numberOfResultDevices + 2));
122 if (resultDevices == NULL)
124 ROS_ERROR(
"LIBUSB - Failed to allocate memory for the device result list.");
125 diagnostics_.
broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
"LIBUSB - Failed to allocate memory for the device result list.");
129 resultDevices[numberOfResultDevices] = devices[i];
130 resultDevices[numberOfResultDevices + 1] = NULL;
131 libusb_ref_device(devices[i]);
132 numberOfResultDevices++;
140 libusb_free_device_list(devices, 1);
145 *list = resultDevices;
146 return numberOfResultDevices;
158 struct libusb_device *dev;
159 while ((dev = list[i++]) != NULL)
160 libusb_unref_device(dev);
170 ROS_INFO(
"Device Class: 0x%x", desc.bDeviceClass);
171 ROS_INFO(
"VendorID: 0x%x", desc.idVendor);
172 ROS_INFO(
"ProductID: 0x%x", desc.idProduct);
180 struct libusb_config_descriptor *config;
185 libusb_get_config_descriptor(device, 0, &config);
187 ROS_INFO(
"Interfaces: %i", (
int)config->bNumInterfaces);
188 ROS_INFO(
"----------------------------------------");
190 const struct libusb_interface *interface;
191 const struct libusb_interface_descriptor *interface_descriptor;
192 const struct libusb_endpoint_descriptor *endpoint_descriptor;
195 for (i = 0; i < config->bNumInterfaces; i++)
197 interface = &config->interface[i];
198 ROS_INFO(
"Number of alternate settings: %i", interface->num_altsetting);
200 for (j = 0; j < interface->num_altsetting; j++)
202 interface_descriptor = &interface->altsetting[j];
204 ROS_INFO(
"Interface number: %i", (
int)interface_descriptor->bInterfaceNumber);
205 ROS_INFO(
"Number of endpoints: %i", (
int)interface_descriptor->bNumEndpoints);
207 for (k = 0; k < interface_descriptor->bNumEndpoints; k++)
209 endpoint_descriptor = &interface_descriptor->endpoint[k];
210 ROS_INFO(
"Descriptor Type: %i", endpoint_descriptor->bDescriptorType);
211 ROS_INFO(
"EP Address: %i", endpoint_descriptor->bEndpointAddress);
215 if (i < config->bNumInterfaces - 1)
217 ROS_INFO(
"----------------------------------------");
224 libusb_free_config_descriptor(config);
233 for (i = 0; i < numberOfDevices; i++)
235 struct libusb_device_descriptor desc;
236 int result = libusb_get_device_descriptor(devices[i], &desc);
239 ROS_ERROR(
"LIBUSB - Failed to get device descriptor");
240 diagnostics_.
broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
"LIBUSB - Failed to get device descriptor.");
245 ROS_INFO(
"SICK AG - TIM3XX - [%zu]", (i + 1));
246 ROS_INFO(
"----------------------------------------");
248 ROS_INFO(
"----------------------------------------");
250 ROS_INFO(
"----------------------------------------");
254 if (numberOfDevices == 0)
256 ROS_INFO(
"LIBUSB - No SICK TIM device connected.");
272 unsigned char receiveBuffer[65536];
277 ROS_DEBUG(
"LIBUSB - Write data... %s", request);
279 int actual_length = 0;
280 int requestLength = strlen(request);
281 result = libusb_bulk_transfer(
device_handle_, (2 | LIBUSB_ENDPOINT_OUT), (
unsigned char*)request, requestLength,
283 if (result != 0 || actual_length != requestLength)
285 ROS_ERROR(
"LIBUSB - Write Error: %i.", result);
293 result = libusb_bulk_transfer(
device_handle_, (1 | LIBUSB_ENDPOINT_IN), receiveBuffer, 65535, &actual_length,
USB_TIMEOUT);
296 ROS_ERROR(
"LIBUSB - Read Error: %i.", result);
301 receiveBuffer[actual_length] = 0;
302 ROS_DEBUG(
"LIBUSB - Read data... %s", receiveBuffer);
305 for(
int i = 0; i < actual_length; i++) {
306 reply->push_back(receiveBuffer[i]);
321 int result = libusb_init(&
ctx_);
324 ROS_ERROR(
"LIBUSB - Initialization failed with the following error code: %i.", result);
332 #if LIBUSB_API_VERSION >= 0x01000106 333 libusb_set_option(
ctx_, LIBUSB_OPTION_LOG_LEVEL, 3);
335 libusb_set_debug(
ctx_, 3);
344 int vendorID = 0x19A2;
345 int deviceID = 0x5001;
353 ROS_ERROR(
"No SICK TiM devices connected!");
360 diagnostics_.
broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
"Chosen SICK TiM scanner not connected");
375 ROS_ERROR(
"LIBUSB - Cannot open device (permission denied?); please read sick_tim/README.md");
376 diagnostics_.
broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
"LIBUSB - Cannot open device (permission denied?); please read sick_tim/README.md");
386 ROS_DEBUG(
"LIBUSB - Kernel driver active");
389 ROS_DEBUG(
"LIBUSB - Kernel driver detached!");
399 ROS_ERROR(
"LIBUSB - Cannot claim interface");
400 diagnostics_.
broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
"LIBUSB - Cannot claim interface.");
405 ROS_INFO(
"LIBUSB - Claimed interface");
413 int result = libusb_bulk_transfer(
device_handle_, (1 | LIBUSB_ENDPOINT_IN), receiveBuffer, bufferSize - 1, actual_length,
417 if (result == LIBUSB_ERROR_TIMEOUT)
419 ROS_WARN(
"LIBUSB - Read Error: LIBUSB_ERROR_TIMEOUT.");
420 diagnostics_.
broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
"LIBUSB - Read Error: LIBUSB_ERROR_TIMEOUT.");
426 ROS_ERROR(
"LIBUSB - Read Error: %i.", result);
432 receiveBuffer[*actual_length] = 0;
virtual int close_device()
void printSOPASDeviceInformation(ssize_t numberOfDevices, libusb_device **devices)
libusb_device ** devices_
void printUSBDeviceDetails(struct libusb_device_descriptor desc)
virtual int get_datagram(unsigned char *receiveBuffer, int bufferSize, int *actual_length)
Read a datagram from the device.
virtual ~SickTimCommonUsb()
virtual int stop_scanner()
virtual int sendSOPASCommand(const char *request, std::vector< unsigned char > *reply)
Send a SOPAS command to the device and print out the response to the console.
libusb_device_handle * device_handle_
ssize_t getSOPASDeviceList(libusb_context *ctx, uint16_t vendorID, uint16_t productID, libusb_device ***list)
diagnostic_updater::Updater diagnostics_
virtual int init_device()
void printUSBInterfaceDetails(libusb_device *device)
SickTimCommonUsb(AbstractParser *parser, int device_number)
void broadcast(int lvl, const std::string msg)
static const unsigned int USB_TIMEOUT
void freeSOPASDeviceList(libusb_device **list)