sick_tim3xx_common_usb.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2013, Osnabrück University
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Osnabrück University nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *  Created on: 24.05.2012
00030  *
00031  *      Authors:
00032  *         Jochen Sprickerhof <jochen@sprickerhof.de>
00033  *         Martin Günther <mguenthe@uos.de>
00034  *
00035  * Based on the TiM communication example by SICK AG.
00036  *
00037  */
00038 
00039 #include <sick_tim3xx/sick_tim3xx_common_usb.h>
00040 
00041 namespace sick_tim3xx
00042 {
00043 
00044 SickTim3xxCommonUsb::SickTim3xxCommonUsb(AbstractParser* parser) : SickTim3xxCommon(parser),
00045     ctx_(NULL), numberOfDevices_(0), devices_(NULL), device_handle_(NULL)
00046 {
00047 }
00048 
00049 SickTim3xxCommonUsb::~SickTim3xxCommonUsb()
00050 {
00051   stop_scanner();
00052   close_device();
00053 }
00054 
00055 int SickTim3xxCommonUsb::close_device()
00056 {
00057   int result = 0;
00058   if (device_handle_ != NULL)
00059   {
00060     /*
00061      * Release the interface
00062      */
00063     result = libusb_release_interface(device_handle_, 0);
00064     if (result != 0)
00065       printf("LIBUSB - Cannot Release Interface!\n");
00066     else
00067       printf("LIBUSB - Released Interface.\n");
00068 
00069     /*
00070      * Close the device handle.
00071      */
00072     libusb_close(device_handle_);
00073   }
00074 
00075   /*
00076    * Free the list of the USB devices.
00077    */
00078   freeSOPASDeviceList(devices_);
00079 
00080   /*
00081    * Close the LIBUSB session.
00082    */
00083   libusb_exit(ctx_);
00084   return result;
00085 }
00086 
00090 ssize_t SickTim3xxCommonUsb::getSOPASDeviceList(libusb_context *ctx, uint16_t vendorID, uint16_t productID,
00091                                              libusb_device ***list)
00092 {
00093   libusb_device **resultDevices = NULL;
00094   ssize_t numberOfResultDevices = 0;
00095   libusb_device **devices;
00096 
00097   /*
00098    * Get a list of all USB devices connected.
00099    */
00100   ssize_t numberOfDevices = libusb_get_device_list(ctx, &devices);
00101 
00102   /*
00103    * Iterate through the list of the connected USB devices and search for devices with the given vendorID and productID.
00104    */
00105   for (ssize_t i = 0; i < numberOfDevices; i++)
00106   {
00107     struct libusb_device_descriptor desc;
00108     int result = libusb_get_device_descriptor(devices[i], &desc);
00109     if (result < 0)
00110     {
00111       ROS_ERROR("LIBUSB - Failed to get device descriptor");
00112       diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Failed to get device descriptor.");
00113       continue;
00114     }
00115 
00116     if (desc.idVendor == vendorID && desc.idProduct == 0x5001)
00117     {
00118       /*
00119        * Add the matching device to the function result list and increase the device reference count.
00120        */
00121       resultDevices = (libusb_device **)realloc(resultDevices, sizeof(libusb_device *) * (numberOfResultDevices + 2));
00122       if (resultDevices == NULL)
00123       {
00124         ROS_ERROR("LIBUSB - Failed to allocate memory for the device result list.");
00125         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Failed to allocate memory for the device result list.");
00126       }
00127       else
00128       {
00129         resultDevices[numberOfResultDevices] = devices[i];
00130         resultDevices[numberOfResultDevices + 1] = NULL;
00131         libusb_ref_device(devices[i]);
00132         numberOfResultDevices++;
00133       }
00134     }
00135   }
00136 
00137   /*
00138    * Free the list of the connected USB devices and decrease the device reference count.
00139    */
00140   libusb_free_device_list(devices, 1);
00141 
00142   /*
00143    * Prepare the return values of the function.
00144    */
00145   *list = resultDevices;
00146   return numberOfResultDevices;
00147 }
00148 
00149 /*
00150  * Free the list of devices obtained from the function 'getSOPASDeviceList'.
00151  */
00152 void SickTim3xxCommonUsb::freeSOPASDeviceList(libusb_device **list)
00153 {
00154   if (!list)
00155     return;
00156 
00157   int i = 0;
00158   struct libusb_device *dev;
00159   while ((dev = list[i++]) != NULL)
00160     libusb_unref_device(dev);
00161 
00162   free(list);
00163 }
00164 
00165 /*
00166  * Print the device details such as USB device class, vendor id and product id to the console.
00167  */
00168 void SickTim3xxCommonUsb::printUSBDeviceDetails(struct libusb_device_descriptor desc)
00169 {
00170   ROS_INFO("Device Class: 0x%x", desc.bDeviceClass);
00171   ROS_INFO("VendorID:     0x%x", desc.idVendor);
00172   ROS_INFO("ProductID:    0x%x", desc.idProduct);
00173 }
00174 
00175 /*
00176  * Iterate through the the interfaces of the USB device and print out the interface details to the console.
00177  */
00178 void SickTim3xxCommonUsb::printUSBInterfaceDetails(libusb_device* device)
00179 {
00180   struct libusb_config_descriptor *config;
00181 
00182   /*
00183    * Get a USB configuration descriptor based on its index.
00184    */
00185   libusb_get_config_descriptor(device, 0, &config);
00186 
00187   ROS_INFO("Interfaces: %i", (int)config->bNumInterfaces);
00188   ROS_INFO("----------------------------------------");
00189 
00190   const struct libusb_interface *interface;
00191   const struct libusb_interface_descriptor *interface_descriptor;
00192   const struct libusb_endpoint_descriptor *endpoint_descriptor;
00193 
00194   int i, j, k;
00195   for (i = 0; i < config->bNumInterfaces; i++)
00196   {
00197     interface = &config->interface[i];
00198     ROS_INFO("Number of alternate settings: %i", interface->num_altsetting);
00199 
00200     for (j = 0; j < interface->num_altsetting; j++)
00201     {
00202       interface_descriptor = &interface->altsetting[j];
00203 
00204       ROS_INFO("Interface number: %i", (int)interface_descriptor->bInterfaceNumber);
00205       ROS_INFO("Number of endpoints: %i", (int)interface_descriptor->bNumEndpoints);
00206 
00207       for (k = 0; k < interface_descriptor->bNumEndpoints; k++)
00208       {
00209         endpoint_descriptor = &interface_descriptor->endpoint[k];
00210         ROS_INFO("Descriptor Type: %i", endpoint_descriptor->bDescriptorType);
00211         ROS_INFO("EP Address: %i", endpoint_descriptor->bEndpointAddress);
00212       }
00213     }
00214 
00215     if (i < config->bNumInterfaces - 1)
00216     {
00217       ROS_INFO("----------------------------------------");
00218     }
00219   }
00220 
00221   /*
00222    * Free the configuration descriptor obtained from 'libusb_get_config_descriptor'
00223    */
00224   libusb_free_config_descriptor(config);
00225 }
00226 
00230 void SickTim3xxCommonUsb::printSOPASDeviceInformation(ssize_t numberOfDevices, libusb_device** devices)
00231 {
00232   ssize_t i;
00233   for (i = 0; i < numberOfDevices; i++)
00234   {
00235     struct libusb_device_descriptor desc;
00236     int result = libusb_get_device_descriptor(devices[i], &desc);
00237     if (result < 0)
00238     {
00239       ROS_ERROR("LIBUSB - Failed to get device descriptor");
00240       diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Failed to get device descriptor.");
00241       continue;
00242     }
00243     if (result == 0)
00244     {
00245       ROS_INFO("SICK AG - TIM3XX - [%zu]", (i + 1));
00246       ROS_INFO("----------------------------------------");
00247       printUSBDeviceDetails(desc);
00248       ROS_INFO("----------------------------------------");
00249       printUSBInterfaceDetails(devices[i]);
00250       ROS_INFO("----------------------------------------");
00251     }
00252   }
00253 
00254   if (numberOfDevices == 0)
00255   {
00256     ROS_INFO("LIBUSB - No SICK TIM3xx device connected.");
00257   }
00258 }
00259 
00263 int SickTim3xxCommonUsb::sendSOPASCommand(const char* request, std::vector<unsigned char> * reply)
00264 {
00265   if (device_handle_ == NULL) {
00266     ROS_ERROR("LIBUSB - device not open");
00267     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - device not open.");
00268     return EXIT_FAILURE;
00269   }
00270 
00271   int result = 0;
00272   unsigned char receiveBuffer[65536];
00273 
00274   /*
00275    * Write a SOPAS variable read request to the device.
00276    */
00277   ROS_DEBUG("LIBUSB - Write data... %s", request);
00278 
00279   int actual_length = 0;
00280   int requestLength = strlen(request);
00281   result = libusb_bulk_transfer(device_handle_, (2 | LIBUSB_ENDPOINT_OUT), (unsigned char*)request, requestLength,
00282                                 &actual_length, 0);
00283   if (result != 0 || actual_length != requestLength)
00284   {
00285     ROS_ERROR("LIBUSB - Write Error: %i.", result);
00286     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Write Error.");
00287     return result;
00288   }
00289 
00290   /*
00291    * Read the SOPAS device response with the given timeout.
00292    */
00293   result = libusb_bulk_transfer(device_handle_, (1 | LIBUSB_ENDPOINT_IN), receiveBuffer, 65535, &actual_length, USB_TIMEOUT);
00294   if (result != 0)
00295   {
00296     ROS_ERROR("LIBUSB - Read Error: %i.", result);
00297     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Read Error.");
00298     return result;
00299   }
00300 
00301   receiveBuffer[actual_length] = 0;
00302   ROS_DEBUG("LIBUSB - Read data...  %s", receiveBuffer);
00303   if(reply) {
00304       reply->clear();
00305       for(int i = 0; i < actual_length; i++) {
00306           reply->push_back(receiveBuffer[i]);
00307       }
00308   }
00309 
00310   return result;
00311 }
00312 
00313 /*
00314  * provided as a separate method (not inside constructor) so we can return error codes
00315  */
00316 int SickTim3xxCommonUsb::init_device()
00317 {
00318   /*
00319    * Create and initialize a new LIBUSB session.
00320    */
00321   int result = libusb_init(&ctx_);
00322   if (result != 0)
00323   {
00324     ROS_ERROR("LIBUSB - Initialization failed with the following error code: %i.", result);
00325     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Initialization failed.");
00326     return EXIT_FAILURE;
00327   }
00328 
00329   /*
00330    * Set the verbosity level to 3 as suggested in the documentation.
00331    */
00332   libusb_set_debug(ctx_, 3);
00333 
00334   /*
00335    * Get a list of all SICK TIM3xx devices connected to the USB bus.
00336    *
00337    * As a shortcut, you can also use the LIBUSB function:
00338    * libusb_open_device_with_vid_pid(ctx, 0x19A2, 0x5001).
00339    */
00340   int vendorID = 0x19A2; // SICK AG
00341   int deviceID = 0x5001; // TIM3XX
00342   numberOfDevices_ = getSOPASDeviceList(ctx_, vendorID, deviceID, &devices_);
00343 
00344   /*
00345    * If available, open the first SICK TIM3xx device.
00346    */
00347   if (numberOfDevices_ == 0)
00348   {
00349     ROS_ERROR("No SICK TiM3xx devices connected!");
00350     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "No SICK TiM3xx devices connected!");
00351     return EXIT_FAILURE;
00352   }
00353   else if (numberOfDevices_ > 1)
00354   {
00355     ROS_WARN("%zu TiM3xx scanners connected, using the first one", numberOfDevices_);
00356     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::WARN, "Multiple TiMxxx scanners connected, using the first one.");
00357   }
00358 
00359   /*
00360    * Print out the SOPAS device information to the console.
00361    */
00362   printSOPASDeviceInformation(numberOfDevices_, devices_);
00363 
00364   /*
00365    * Open the device handle and detach all kernel drivers.
00366    */
00367   libusb_open(devices_[0], &device_handle_);
00368   if (device_handle_ == NULL)
00369   {
00370     ROS_ERROR("LIBUSB - Cannot open device; please read sick_tim3xx/udev/README");
00371     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Cannot open device; please read sick_tim3xx/udev/README.");
00372     return EXIT_FAILURE;
00373   }
00374   else
00375   {
00376     ROS_DEBUG("LIBUSB - Device opened");
00377   }
00378 
00379   if (libusb_kernel_driver_active(device_handle_, 0) == 1)
00380   {
00381     ROS_DEBUG("LIBUSB - Kernel driver active");
00382     if (libusb_detach_kernel_driver(device_handle_, 0) == 0)
00383     {
00384       ROS_DEBUG("LIBUSB - Kernel driver detached!");
00385     }
00386   }
00387 
00388   /*
00389    * Claim the interface 0
00390    */
00391   result = libusb_claim_interface(device_handle_, 0);
00392   if (result < 0)
00393   {
00394     ROS_ERROR("LIBUSB - Cannot claim interface");
00395     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Cannot claim interface.");
00396     return EXIT_FAILURE;
00397   }
00398   else
00399   {
00400     ROS_INFO("LIBUSB - Claimed interface");
00401   }
00402 
00403   return EXIT_SUCCESS;
00404 }
00405 
00406 int SickTim3xxCommonUsb::get_datagram(unsigned char* receiveBuffer, int bufferSize, int* actual_length)
00407 {
00408   int result = libusb_bulk_transfer(device_handle_, (1 | LIBUSB_ENDPOINT_IN), receiveBuffer, bufferSize - 1, actual_length,
00409                                 USB_TIMEOUT);   // read up to bufferSize - 1 to leave space for \0
00410   if (result != 0)
00411   {
00412     if (result == LIBUSB_ERROR_TIMEOUT)
00413     {
00414       ROS_WARN("LIBUSB - Read Error: LIBUSB_ERROR_TIMEOUT.");
00415       diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Read Error: LIBUSB_ERROR_TIMEOUT.");
00416       *actual_length = 0;
00417       return EXIT_SUCCESS; // return success with size 0 to continue looping
00418     }
00419     else
00420     {
00421       ROS_ERROR("LIBUSB - Read Error: %i.", result);
00422       diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Read Error.");
00423       return result; // return failure to exit node
00424     }
00425   }
00426 
00427   receiveBuffer[*actual_length] = 0;
00428   return EXIT_SUCCESS;
00429 }
00430 
00431 } /* namespace sick_tim3xx */


sick_tim3xx
Author(s): Jochen Sprickerhof, Martin Günther
autogenerated on Fri Aug 15 2014 11:54:25