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