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-2009 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 00037 using namespace qrk; 00038 using namespace fawkes; 00039 00040 00041 00042 /** @class HokuyoUrgAcquisitionThread "urg_aqt.h" 00043 * Laser acqusition thread for Hokuyo URG laser range finders. 00044 * This thread fetches the data from the laser. 00045 * @author Tim Niemueller 00046 */ 00047 00048 00049 /** Constructor. 00050 * @param cfg_name short name of configuration group 00051 * @param cfg_prefix configuration path prefix 00052 */ 00053 HokuyoUrgAcquisitionThread::HokuyoUrgAcquisitionThread(std::string &cfg_name, 00054 std::string &cfg_prefix) 00055 : LaserAcquisitionThread("HokuyoUrgAcquisitionThread") 00056 { 00057 set_name("HokuyoURG(%s)", cfg_name.c_str()); 00058 __pre_init_done = false; 00059 __cfg_name = cfg_name; 00060 __cfg_prefix = cfg_prefix; 00061 } 00062 00063 00064 void 00065 HokuyoUrgAcquisitionThread::pre_init(fawkes::Configuration *config, 00066 fawkes::Logger *logger) 00067 { 00068 if (__pre_init_done) return; 00069 00070 __number_of_values = _distances_size = 360; 00071 00072 __pre_init_done = true; 00073 } 00074 00075 void 00076 HokuyoUrgAcquisitionThread::init() 00077 { 00078 pre_init(config, logger); 00079 00080 __cfg_device = config->get_string((__cfg_prefix + "device").c_str()); 00081 00082 __ctrl = new UrgCtrl(); 00083 std::auto_ptr<UrgCtrl> ctrl(__ctrl); 00084 if ( ! __ctrl->connect(__cfg_device.c_str()) ) { 00085 throw Exception("Connecting to URG laser failed: %s", __ctrl->what()); 00086 } 00087 00088 __ctrl->setCaptureMode(AutoCapture); 00089 00090 std::vector<std::string> version_info; 00091 if (__ctrl->versionLines(version_info)) { 00092 for (unsigned int i = 0; i < version_info.size(); ++i) { 00093 std::string::size_type colon_idx = version_info[i].find(":"); 00094 std::string::size_type semi_colon_idx = version_info[i].find(";"); 00095 if ((colon_idx == std::string::npos) || 00096 (semi_colon_idx == std::string::npos)) { 00097 logger->log_warn(name(), "Could not understand version info string '%s'", 00098 version_info[i].c_str()); 00099 } else { 00100 std::string::size_type val_len = semi_colon_idx - colon_idx - 1; 00101 std::string key = version_info[i].substr(0, colon_idx); 00102 std::string value = version_info[i].substr(colon_idx+1, val_len); 00103 __device_info[key] = value; 00104 logger->log_info(name(), "%s: %s", key.c_str(), value.c_str()); 00105 } 00106 } 00107 } else { 00108 throw Exception("Failed retrieving version info from device: %s", __ctrl->what()); 00109 } 00110 00111 if (__device_info.find("PROD") == __device_info.end()) { 00112 throw Exception("Failed to read product info for URG laser"); 00113 } 00114 00115 int scan_msec = __ctrl->scanMsec(); 00116 00117 try { 00118 __first_ray = config->get_uint((__cfg_prefix + "first_ray").c_str()); 00119 __last_ray = config->get_uint((__cfg_prefix + "last_ray").c_str()); 00120 __front_ray = config->get_uint((__cfg_prefix + "front_ray").c_str()); 00121 __slit_division = config->get_uint((__cfg_prefix + "slit_division").c_str()); 00122 } catch (Exception &e) { 00123 logger->log_info(name(), "No or incomplete config data, reading from device"); 00124 // Get data from device 00125 RangeSensorParameter p = __ctrl->parameter(); 00126 __first_ray = p.area_min; 00127 __last_ray = p.area_max; 00128 __front_ray = p.area_front; 00129 __slit_division = p.area_total; 00130 } 00131 00132 __step_per_angle = __slit_division / 360.; 00133 __angle_per_step = 360. / __slit_division; 00134 __angular_range = (__last_ray - __first_ray) * __angle_per_step; 00135 00136 logger->log_info(name(), "Time per scan: %i msec", scan_msec); 00137 logger->log_info(name(), "Rays range: %u..%u, front at %u", 00138 __first_ray, __last_ray, __front_ray); 00139 logger->log_info(name(), "Slit Division: %u", __slit_division); 00140 logger->log_info(name(), "Step/Angle: %f", __step_per_angle); 00141 logger->log_info(name(), "Angle/Step: %f deg", __angle_per_step); 00142 logger->log_info(name(), "Angular Range: %f deg", __angular_range); 00143 00144 // that should be 1000 really to convert msec -> usec. But empirically 00145 // the results are slightly better with 990 as factor. 00146 __timer = new TimeWait(clock, scan_msec * 990); 00147 00148 alloc_distances(__number_of_values); 00149 00150 ctrl.release(); 00151 } 00152 00153 00154 void 00155 HokuyoUrgAcquisitionThread::finalize() 00156 { 00157 free(_distances); 00158 _distances = NULL; 00159 delete __timer; 00160 00161 __ctrl->stop(); 00162 delete __ctrl; 00163 00164 logger->log_debug(name(), "Stopping laser"); 00165 } 00166 00167 00168 void 00169 HokuyoUrgAcquisitionThread::loop() 00170 { 00171 __timer->mark_start(); 00172 00173 std::vector<long> values; 00174 int num_values = __ctrl->capture(values); 00175 if (num_values > 0) { 00176 //logger->log_debug(name(), "Captured %i values", num_values); 00177 _data_mutex->lock(); 00178 00179 _new_data = true; 00180 for (unsigned int a = 0; a < 360; ++a) { 00181 unsigned int front_idx = __front_ray + roundf(a * __step_per_angle); 00182 unsigned int idx = front_idx % __slit_division; 00183 if ( (idx >= __first_ray) && (idx <= __last_ray) ) { 00184 // div by 1000.f: mm -> m 00185 _distances[a] = values[idx] / 1000.f; 00186 } 00187 } 00188 _data_mutex->unlock(); 00189 //} else { 00190 //logger->log_warn(name(), "No new scan available, ignoring"); 00191 } 00192 00193 __timer->wait(); 00194 }