Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * urg_gbx_aqt.cpp - Thread for Hokuyo URG using the Gearbox library 00004 * 00005 * Created: Fri Dec 04 20:47:50 2009 (at Frankfurt Airport) 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_gbx_aqt.h" 00024 00025 #include <core/threading/mutex.h> 00026 00027 #ifdef HAVE_URG_GBX_9_11 00028 # include <hokuyo_aist/hokuyo_aist.h> 00029 #else 00030 # include <hokuyoaist/hokuyoaist.h> 00031 #endif 00032 #include <flexiport/flexiport.h> 00033 00034 #include <memory> 00035 #include <cstdlib> 00036 #include <cmath> 00037 #include <string> 00038 #include <cstdio> 00039 00040 #ifdef HAVE_URG_GBX_9_11 00041 using namespace hokuyo_aist; 00042 #else 00043 using namespace hokuyoaist; 00044 #endif 00045 using namespace fawkes; 00046 00047 00048 /** @class HokuyoUrgGbxAcquisitionThread "urg_gbx_aqt.h" 00049 * Laser acqusition thread for Hokuyo URG laser range finders. 00050 * This thread fetches the data from the laser. This implementation uses 00051 * the Gearbox library. 00052 * @author Tim Niemueller 00053 */ 00054 00055 00056 /** Constructor. 00057 * @param cfg_name short name of configuration group 00058 * @param cfg_prefix configuration path prefix 00059 */ 00060 HokuyoUrgGbxAcquisitionThread::HokuyoUrgGbxAcquisitionThread(std::string &cfg_name, 00061 std::string &cfg_prefix) 00062 : LaserAcquisitionThread("HokuyoUrgGbxAcquisitionThread") 00063 { 00064 set_name("HokuyoURG_GBX(%s)", cfg_name.c_str()); 00065 __pre_init_done = false; 00066 __cfg_name = cfg_name; 00067 __cfg_prefix = cfg_prefix; 00068 } 00069 00070 00071 void 00072 HokuyoUrgGbxAcquisitionThread::pre_init(fawkes::Configuration *config, 00073 fawkes::Logger *logger) 00074 { 00075 if (__pre_init_done) return; 00076 00077 __number_of_values = _distances_size = 360; 00078 00079 __pre_init_done = true; 00080 } 00081 00082 void 00083 HokuyoUrgGbxAcquisitionThread::init() 00084 { 00085 pre_init(config, logger); 00086 00087 __cfg_device = config->get_string((__cfg_prefix + "device").c_str()); 00088 00089 #ifdef HAVE_URG_GBX_9_11 00090 __laser = new HokuyoLaser(); 00091 std::auto_ptr<HokuyoLaser> laser(__laser); 00092 #else 00093 __laser = new Sensor(); 00094 std::auto_ptr<Sensor> laser(__laser); 00095 #endif 00096 std::string port_options = "type=serial,device=" + __cfg_device + ",timeout=1"; 00097 try { 00098 #ifdef HAVE_URG_GBX_9_11 00099 __laser->Open(port_options); 00100 #else 00101 __laser->open(port_options); 00102 #endif 00103 } catch (flexiport::PortException &e) { 00104 throw Exception("Connecting to URG laser failed: %s", e.what()); 00105 } 00106 00107 #ifdef HAVE_URG_GBX_9_11 00108 HokuyoSensorInfo info; 00109 __laser->GetSensorInfo(&info); 00110 00111 __data = new HokuyoData(); 00112 __first_ray = info.firstStep; 00113 __last_ray = info.lastStep; 00114 __front_ray = info.frontStep; 00115 00116 #else 00117 SensorInfo info; 00118 __laser->get_sensor_info(info); 00119 __data = new ScanData(); 00120 00121 __first_ray = info.first_step; 00122 __last_ray = info.last_step; 00123 __front_ray = info.front_step; 00124 #endif 00125 00126 __slit_division = info.steps; 00127 __num_rays = __last_ray - __first_ray; 00128 __front_idx = __front_ray - __first_ray; 00129 00130 __step_per_angle = __slit_division / 360.; 00131 __angle_per_step = 360. / __slit_division; 00132 __angular_range = (__last_ray - __first_ray) * __angle_per_step; 00133 00134 logger->log_info(name(), "VEND: %s", info.vendor.c_str()); 00135 logger->log_info(name(), "PROD: %s", info.product.c_str()); 00136 logger->log_info(name(), "FIRM: %s", info.firmware.c_str()); 00137 logger->log_info(name(), "PROT: %s", info.protocol.c_str()); 00138 logger->log_info(name(), "SERI: %s", info.serial.c_str()); 00139 logger->log_info(name(), "Rays range: %u..%u, front at %u (idx %u), " 00140 "%u rays total", __first_ray, __last_ray, __front_ray, 00141 __front_idx, __num_rays); 00142 logger->log_info(name(), "Slit Division: %u", __slit_division); 00143 logger->log_info(name(), "Step/Angle: %f", __step_per_angle); 00144 logger->log_info(name(), "Angle/Step: %f deg", __angle_per_step); 00145 logger->log_info(name(), "Angular Range: %f deg", __angular_range); 00146 00147 alloc_distances(__number_of_values); 00148 #ifdef HAVE_URG_GBX_9_11 00149 __laser->SetPower(true); 00150 #else 00151 __laser->set_power(true); 00152 #endif 00153 00154 laser.release(); 00155 } 00156 00157 00158 void 00159 HokuyoUrgGbxAcquisitionThread::finalize() 00160 { 00161 free(_distances); 00162 _distances = NULL; 00163 00164 logger->log_debug(name(), "Stopping laser"); 00165 #ifdef HAVE_URG_GBX_9_11 00166 __laser->SetPower(false); 00167 #else 00168 __laser->set_power(false); 00169 #endif 00170 delete __laser; 00171 delete __data; 00172 } 00173 00174 00175 void 00176 HokuyoUrgGbxAcquisitionThread::loop() 00177 { 00178 // static Time ref(clock); 00179 // static Time now(clock); 00180 // static unsigned int scans = 0; 00181 00182 // now.stamp(); 00183 // if (now - &ref >= 1) { 00184 // logger->log_debug(name(), "Current: %u scans/sec", scans); 00185 // scans = 0; 00186 // ref = now; 00187 // } else { 00188 // ++scans; 00189 // } 00190 00191 try { 00192 // GetNewRanges is causes scans/sec to be halfed 00193 #ifdef HAVE_URG_GBX_9_11 00194 __laser->GetRanges(__data); 00195 } catch (HokuyoError &he) { 00196 #else 00197 __laser->get_ranges(*__data); 00198 } catch (BaseError &he) { 00199 #endif 00200 logger->log_warn(name(), "Failed to read data: %s", he.what()); 00201 return; 00202 } 00203 00204 #ifdef HAVE_URG_GBX_9_11 00205 const uint32_t *ranges = __data->Ranges(); 00206 #else 00207 const uint32_t *ranges = __data->ranges(); 00208 #endif 00209 00210 _data_mutex->lock(); 00211 00212 _new_data = true; 00213 for (unsigned int a = 0; a < 360; ++a) { 00214 unsigned int frontrel_idx = __front_idx + roundf(a * __step_per_angle); 00215 unsigned int idx = frontrel_idx % __slit_division; 00216 if ( idx <= __num_rays ) { 00217 // div by 1000.f: mm -> m 00218 _distances[a] = ranges[idx] / 1000.f; 00219 } 00220 } 00221 _data_mutex->unlock(); 00222 }