sick_tim_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_tim/sick_tim_common_usb.h>
00040 
00041 namespace sick_tim
00042 {
00043 
00044 SickTimCommonUsb::SickTimCommonUsb(AbstractParser* parser, int device_number) : SickTimCommon(parser),
00045     ctx_(NULL), numberOfDevices_(0), devices_(NULL), device_handle_(NULL), device_number_(device_number)
00046 {
00047 }
00048 
00049 SickTimCommonUsb::~SickTimCommonUsb()
00050 {
00051   stop_scanner();
00052   close_device();
00053 }
00054 
00055 int SickTimCommonUsb::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 SickTimCommonUsb::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 SickTimCommonUsb::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 SickTimCommonUsb::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 SickTimCommonUsb::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 SickTimCommonUsb::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 TIM device connected.");
00257   }
00258 }
00259 
00263 int SickTimCommonUsb::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 ExitError;
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 SickTimCommonUsb::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 ExitError;
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 TiM devices connected!");
00350     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "No SICK TiM devices connected!");
00351     return ExitError;
00352   }
00353   else if (numberOfDevices_ <= device_number_)
00354   {
00355     ROS_ERROR("Device number %d too high, only %zu SICK TiM scanners connected", device_number_, numberOfDevices_);
00356     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Chosen SICK TiM scanner not connected");
00357         return ExitError;
00358   }
00359 
00360   /*
00361    * Print out the SOPAS device information to the console.
00362    */
00363   printSOPASDeviceInformation(numberOfDevices_, devices_);
00364 
00365   /*
00366    * Open the device handle and detach all kernel drivers.
00367    */
00368   libusb_open(devices_[device_number_], &device_handle_);
00369   if (device_handle_ == NULL)
00370   {
00371     ROS_ERROR("LIBUSB - Cannot open device (permission denied?); please read sick_tim/README.md");
00372     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Cannot open device (permission denied?); please read sick_tim/README.md");
00373     return ExitError;
00374   }
00375   else
00376   {
00377     ROS_DEBUG("LIBUSB - Device opened");
00378   }
00379 
00380   if (libusb_kernel_driver_active(device_handle_, 0) == 1)
00381   {
00382     ROS_DEBUG("LIBUSB - Kernel driver active");
00383     if (libusb_detach_kernel_driver(device_handle_, 0) == 0)
00384     {
00385       ROS_DEBUG("LIBUSB - Kernel driver detached!");
00386     }
00387   }
00388 
00389   /*
00390    * Claim the interface 0
00391    */
00392   result = libusb_claim_interface(device_handle_, 0);
00393   if (result < 0)
00394   {
00395     ROS_ERROR("LIBUSB - Cannot claim interface");
00396     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Cannot claim interface.");
00397     return ExitError;
00398   }
00399   else
00400   {
00401     ROS_INFO("LIBUSB - Claimed interface");
00402   }
00403 
00404   return ExitSuccess;
00405 }
00406 
00407 int SickTimCommonUsb::get_datagram(unsigned char* receiveBuffer, int bufferSize, int* actual_length)
00408 {
00409   int result = libusb_bulk_transfer(device_handle_, (1 | LIBUSB_ENDPOINT_IN), receiveBuffer, bufferSize - 1, actual_length,
00410                                 USB_TIMEOUT);   // read up to bufferSize - 1 to leave space for \0
00411   if (result != 0)
00412   {
00413     if (result == LIBUSB_ERROR_TIMEOUT)
00414     {
00415       ROS_WARN("LIBUSB - Read Error: LIBUSB_ERROR_TIMEOUT.");
00416       diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Read Error: LIBUSB_ERROR_TIMEOUT.");
00417       *actual_length = 0;
00418       return ExitSuccess; // return success with size 0 to continue looping
00419     }
00420     else
00421     {
00422       ROS_ERROR("LIBUSB - Read Error: %i.", result);
00423       diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "LIBUSB - Read Error.");
00424       return result; // return failure to exit node
00425     }
00426   }
00427 
00428   receiveBuffer[*actual_length] = 0;
00429   return ExitSuccess;
00430 }
00431 
00432 } /* namespace sick_tim */


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Thu May 9 2019 02:32:02