Fawkes API  Fawkes Development Version
urg_aqt.cpp
00001 
00002 /***************************************************************************
00003  *  urg_aqt.cpp - Thread to retrieve laser data from Hokuyo URG
00004  *
00005  *  Created: Sat Nov 28 01:31:26 2009
00006  *  Copyright  2008-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "urg_aqt.h"
00024 
00025 #include <core/threading/mutex.h>
00026 #include <utils/time/wait.h>
00027 
00028 #include <urg/UrgCtrl.h>
00029 #include <urg/RangeSensorParameter.h>
00030 
00031 #include <memory>
00032 #include <cstdlib>
00033 #include <cmath>
00034 #include <string>
00035 #include <cstdio>
00036 #include <cerrno>
00037 #include <sys/file.h>
00038 #include <unistd.h>
00039 #include <limits>
00040 #ifdef HAVE_LIBUDEV
00041 #  include <cstring>
00042 #  include <libudev.h>
00043 #endif
00044 
00045 using namespace qrk;
00046 using namespace fawkes;
00047 
00048 
00049 
00050 /** @class HokuyoUrgAcquisitionThread "urg_aqt.h"
00051  * Laser acqusition thread for Hokuyo URG laser range finders.
00052  * This thread fetches the data from the laser.
00053  * @author Tim Niemueller
00054  */
00055 
00056 
00057 /** Constructor.
00058  * @param cfg_name short name of configuration group
00059  * @param cfg_prefix configuration path prefix
00060  */
00061 HokuyoUrgAcquisitionThread::HokuyoUrgAcquisitionThread(std::string &cfg_name,
00062                                                        std::string &cfg_prefix)
00063   : LaserAcquisitionThread("HokuyoUrgAcquisitionThread")
00064 {
00065   set_name("HokuyoURG(%s)", cfg_name.c_str());
00066   __pre_init_done = false;
00067   __cfg_name   = cfg_name;
00068   __cfg_prefix = cfg_prefix;
00069 }
00070 
00071 
00072 void
00073 HokuyoUrgAcquisitionThread::pre_init(fawkes::Configuration *config,
00074                                      fawkes::Logger        *logger)
00075 {
00076   if (__pre_init_done)  return;
00077 
00078   __number_of_values = _distances_size = 360;
00079 
00080   __pre_init_done = true;
00081 }
00082 
00083 void
00084 HokuyoUrgAcquisitionThread::init()
00085 {
00086   pre_init(config, logger);
00087 
00088 #ifdef HAVE_LIBUDEV
00089   try {
00090     __cfg_device = config->get_string((__cfg_prefix + "device").c_str());
00091   } catch (Exception &e) {
00092     // check if bus/port numbers are given
00093     try {
00094       __cfg_device = "";
00095       __cfg_serial = config->get_string((__cfg_prefix + "serial").c_str());
00096 
00097       // try to find device using udev
00098       struct udev *udev;
00099       struct udev_enumerate *enumerate;
00100       struct udev_list_entry *devices, *dev_list_entry;
00101       struct udev_device *dev, *usb_device;
00102       udev = udev_new();
00103       if (!udev) {
00104         throw Exception("HokuyoURG: Failed to initialize udev for "
00105                         "device detection");
00106       }
00107 
00108       enumerate = udev_enumerate_new(udev);
00109       udev_enumerate_add_match_subsystem(enumerate, "tty");
00110       udev_enumerate_scan_devices(enumerate);
00111 
00112       devices = udev_enumerate_get_list_entry(enumerate);
00113       udev_list_entry_foreach(dev_list_entry, devices) {
00114 
00115         const char *path;
00116 
00117         path = udev_list_entry_get_name(dev_list_entry);
00118         dev = udev_device_new_from_syspath(udev, path);
00119 
00120         usb_device = udev_device_get_parent_with_subsystem_devtype(dev, "usb",
00121                                                                    "usb_device");
00122         if (! dev || ! usb_device) continue;
00123         
00124         if ( (strcmp(udev_device_get_sysattr_value(usb_device,"manufacturer"),
00125                      "Hokuyo Data Flex for USB") == 0) &&
00126              (strcmp(udev_device_get_sysattr_value(usb_device,"product"),
00127                      "URG-Series USB Driver") == 0) )
00128         {
00129 
00130           const char *devpath = udev_device_get_devnode(dev);
00131           int urgfd = open(devpath, 0, O_RDONLY);
00132           if (urgfd == -1) {
00133             logger->log_info(name(), "Failed to probe %s, cannot open file: %s",
00134                              devpath, strerror(errno));
00135             continue;
00136           }
00137           if (flock(urgfd, LOCK_EX | LOCK_NB) != 0) {
00138             logger->log_info(name(), "Failed to probe %s, cannot lock file: %s",
00139                              devpath, strerror(errno));
00140             close(urgfd);
00141             continue;
00142           }
00143           UrgCtrl probe_ctrl;
00144           if ( ! probe_ctrl.connect(devpath) )  {
00145             logger->log_info(name(), "Failed to probe %s: %s", devpath,
00146                              probe_ctrl.what());
00147             flock(urgfd, LOCK_UN);
00148             close(urgfd);
00149             continue;
00150           }
00151 
00152           std::map<std::string, std::string> devinfo;
00153           try {
00154             devinfo = get_device_info(&probe_ctrl);
00155           } catch (Exception &e) {
00156             logger->log_info(name(), "Failed to probe device info %s: %s",
00157                              devpath, e.what());
00158             flock(urgfd, LOCK_UN);
00159             close(urgfd);
00160             continue;
00161           }
00162           flock(urgfd, LOCK_UN);
00163           close(urgfd);
00164 
00165           if (devinfo["SERI"] == __cfg_serial) {
00166             __cfg_device = devpath;
00167 
00168             logger->log_info(
00169               name(), "Matching URG at %s (vendor: %s (%s), "
00170               "product: %s (%s), serial %s)", devpath,
00171               udev_device_get_sysattr_value(usb_device, "manufacturer"),
00172               udev_device_get_sysattr_value(usb_device, "idVendor"),
00173               udev_device_get_sysattr_value(usb_device, "product"),
00174               udev_device_get_sysattr_value(usb_device, "idProduct"),
00175               devinfo["SERI"].c_str());
00176 
00177             break;
00178           } else {
00179             logger->log_info(name(), "Non-matching URG with serial %s at %s",
00180                              devinfo["SERI"].c_str(), devpath);
00181           }
00182         }
00183       }
00184       udev_enumerate_unref(enumerate);
00185       udev_unref(udev);
00186 
00187       if (__cfg_device == "") {
00188         throw Exception("No Hokuyo URG with serial %s found",
00189                         __cfg_serial.c_str());
00190       }
00191 
00192     } catch (Exception &e2) {
00193       e.append(e2);
00194       throw e;
00195     }
00196   }
00197 #else
00198   __cfg_device = config->get_string((__cfg_prefix + "device").c_str());
00199 #endif
00200 
00201   __ctrl = new UrgCtrl();
00202   std::auto_ptr<UrgCtrl> ctrl(__ctrl);
00203   __fd = open(__cfg_device.c_str(), 0, O_RDONLY);
00204   if (__fd == -1) {
00205     throw Exception(errno, "Failed to open URG device %s", __cfg_device.c_str());
00206   }
00207   if (flock(__fd, LOCK_EX | LOCK_NB) != 0) {
00208     close(__fd);
00209     throw Exception("Failed to acquire lock for URG device %s", __cfg_device.c_str());
00210   }
00211   if ( ! __ctrl->connect(__cfg_device.c_str()) ) {
00212     close(__fd);
00213     flock(__fd, LOCK_UN);
00214     throw Exception("Connecting to URG laser failed: %s", __ctrl->what());
00215   }
00216 
00217   __ctrl->setCaptureMode(AutoCapture);
00218   __device_info = get_device_info(__ctrl);
00219 
00220   if (__device_info.find("PROD") == __device_info.end()) {
00221     close(__fd);
00222     flock(__fd, LOCK_UN);
00223     throw Exception("Failed to read product info for URG laser");
00224   }
00225 
00226   logger->log_info(name(), "Using device file %s", __cfg_device.c_str());
00227   std::map<std::string, std::string>::iterator di;
00228   for (di = __device_info.begin(); di != __device_info.end(); ++di) {
00229     logger->log_info(name(), "%s: %s", di->first.c_str(), di->second.c_str());
00230   }
00231 
00232   int scan_msec = __ctrl->scanMsec();
00233   float distance_min = 0.;
00234   float distance_max = 0.;
00235 
00236   try {
00237     __first_ray     = config->get_uint((__cfg_prefix + "first_ray").c_str());
00238     __last_ray      = config->get_uint((__cfg_prefix + "last_ray").c_str());
00239     __front_ray     = config->get_uint((__cfg_prefix + "front_ray").c_str());
00240     __slit_division = config->get_uint((__cfg_prefix + "slit_division").c_str());
00241   } catch (Exception &e) {
00242     logger->log_info(name(), "No or incomplete config data, reading from device");
00243     // Get data from device
00244     RangeSensorParameter p = __ctrl->parameter();
00245     __first_ray     = p.area_min;
00246     __last_ray      = p.area_max;
00247     __front_ray     = p.area_front;
00248     __slit_division = p.area_total;
00249     distance_min    = p.distance_min / 1000.;
00250     distance_max    = p.distance_max / 1000.;
00251   }
00252 
00253   __step_per_angle = __slit_division / 360.;
00254   __angle_per_step = 360. / __slit_division;
00255   __angular_range  = (__last_ray - __first_ray) * __angle_per_step;
00256 
00257   logger->log_info(name(), "Time per scan: %i msec", scan_msec);
00258   logger->log_info(name(), "Rays range:    %u..%u, front at %u",
00259                    __first_ray, __last_ray, __front_ray);
00260   logger->log_info(name(), "Slit Division: %u", __slit_division);
00261   logger->log_info(name(), "Step/Angle:    %f", __step_per_angle);
00262   logger->log_info(name(), "Angle/Step:    %f deg", __angle_per_step);
00263   logger->log_info(name(), "Angular Range: %f deg", __angular_range);
00264   logger->log_info(name(), "Min dist:      %f m", distance_min);
00265   logger->log_info(name(), "Max dist:      %f m", distance_max);
00266 
00267   // that should be 1000 really to convert msec -> usec. But empirically
00268   // the results are slightly better with 990 as factor.
00269   __timer = new TimeWait(clock, scan_msec * 990);
00270 
00271   alloc_distances(__number_of_values);
00272 
00273   ctrl.release();
00274 }
00275 
00276 
00277 void
00278 HokuyoUrgAcquisitionThread::finalize()
00279 {
00280   free(_distances);
00281   _distances = NULL;
00282   delete __timer;
00283 
00284   __ctrl->stop();
00285   delete __ctrl;
00286 
00287   close(__fd);
00288   flock(__fd, LOCK_UN);
00289 
00290   logger->log_debug(name(), "Stopping laser");
00291 }
00292 
00293 
00294 void
00295 HokuyoUrgAcquisitionThread::loop()
00296 {
00297   __timer->mark_start();
00298 
00299   std::vector<long> values;
00300   int num_values = __ctrl->capture(values);
00301   if (num_values > 0) {
00302     //logger->log_debug(name(), "Captured %i values", num_values);
00303     _data_mutex->lock();
00304 
00305     _new_data = true;
00306     for (unsigned int a = 0; a < 360; ++a) {
00307       unsigned int front_idx = __front_ray + roundf(a * __step_per_angle);
00308       unsigned int idx = front_idx % __slit_division;
00309       if ( (idx >= __first_ray) && (idx <= __last_ray) ) {
00310         switch (values[idx]) // See the SCIP2.0 reference on page 12, Table 3
00311         {
00312         case 0: // Detected object is possibly at 22m
00313           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00314           break;
00315         case 1: // Reflected light has low intensity
00316           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00317           break;
00318         case 2: // Reflected light has low intensity
00319           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00320           break;
00321         case 6: // Others
00322           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00323           break;
00324         case 7: // Distance data on the preceding and succeeding steps have errors
00325           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00326           break;
00327         case 8: // Intensity difference of two waves
00328           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00329           break;
00330         case 9: // The same step had error in the last two scan
00331           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00332           break;
00333         case 10: // Others
00334           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00335           break;
00336         case 11: // Others
00337           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00338           break;
00339         case 12: // Others
00340           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00341           break;
00342         case 13: // Others
00343           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00344           break;
00345         case 14: // Others
00346           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00347           break;
00348         case 15: // Others
00349           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00350           break;
00351         case 16: // Others
00352           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00353           break;
00354         case 17: // Others
00355           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00356           break;
00357         case 18: // Error reading due to strong reflective object
00358           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00359           break;
00360         case 19: // Non-Measurable step
00361           _distances[a] = std::numeric_limits<float>::quiet_NaN();
00362           break;
00363         default:
00364           // div by 1000.f: mm -> m
00365           _distances[a] = values[idx] / 1000.f;
00366         }
00367       }
00368     }
00369     _data_mutex->unlock();
00370   //} else {
00371     //logger->log_warn(name(), "No new scan available, ignoring");
00372   }
00373 
00374   __timer->wait();
00375 }
00376 
00377 std::map<std::string, std::string>
00378   HokuyoUrgAcquisitionThread::get_device_info(qrk::UrgCtrl *ctrl)
00379 {
00380   std::map<std::string, std::string> device_info;
00381 
00382   std::vector<std::string> version_info;
00383   if (ctrl->versionLines(version_info)) {
00384     for (unsigned int i = 0; i < version_info.size(); ++i) {
00385       std::string::size_type colon_idx      = version_info[i].find(":");
00386       std::string::size_type semi_colon_idx = version_info[i].find(";");
00387       if ((colon_idx == std::string::npos) ||
00388           (semi_colon_idx == std::string::npos)) {
00389         logger->log_warn(name(), "Could not understand version info string '%s'",
00390                          version_info[i].c_str());
00391       } else {
00392         std::string::size_type val_len = semi_colon_idx - colon_idx - 1;
00393         std::string key   = version_info[i].substr(0, colon_idx);
00394         std::string value = version_info[i].substr(colon_idx+1, val_len);
00395         device_info[key] = value;
00396       }
00397     }
00398   } else {
00399     throw Exception("Failed retrieving version info: %s", ctrl->what());
00400   }
00401   return device_info;
00402 }