urg_aqt.cpp

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 }