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