Fawkes API
Fawkes Development Version
|
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 }