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 #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 }

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1