Fawkes API  Fawkes Development Version
sensor_thread.cpp
00001 
00002 /***************************************************************************
00003  *  sensor_thread.cpp - Laser thread that puses data into the interface
00004  *
00005  *  Created: Wed Oct 08 13:32:57 2008
00006  *  Copyright  2006-2008  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 "sensor_thread.h"
00024 #include "acquisition_thread.h"
00025 
00026 #include <interfaces/Laser360Interface.h>
00027 #include <interfaces/Laser720Interface.h>
00028 
00029 using namespace fawkes;
00030 
00031 /** @class LaserSensorThread "sensor_thread.h"
00032  * Laser sensor thread.
00033  * This thread integrates into the Fawkes main loop at the sensor hook and
00034  * publishes new data when available from the LaserAcquisitionThread.
00035  * @author Tim Niemueller
00036  */
00037 
00038 
00039 /** Constructor.
00040  * @param cfg_name short name of configuration group
00041  * @param cfg_prefix configuration path prefix
00042  * @param aqt LaserAcquisitionThread to get data from
00043  */
00044 LaserSensorThread::LaserSensorThread(std::string &cfg_name,
00045                                      std::string &cfg_prefix,
00046                                      LaserAcquisitionThread *aqt)
00047   : Thread("LaserSensorThread", Thread::OPMODE_WAITFORWAKEUP),
00048     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
00049 {
00050   set_name("LaserSensorThread(%s)", cfg_name.c_str());
00051   __aqt        = aqt;
00052   __cfg_name   = cfg_name;
00053   __cfg_prefix = cfg_prefix;
00054 }
00055 
00056 
00057 void
00058 LaserSensorThread::init()
00059 {
00060   __laser360_if = NULL;
00061   __laser720_if = NULL;
00062 
00063   bool main_sensor  = false;
00064 
00065   __cfg_frame = config->get_string((__cfg_prefix + "frame").c_str());
00066 
00067   try {
00068     main_sensor = config->get_bool((__cfg_prefix + "main_sensor").c_str());
00069   } catch (Exception &e) {} // ignored, assume no
00070 
00071   __aqt->pre_init(config, logger);
00072 
00073   __num_values = __aqt->get_distance_data_size();
00074 
00075   std::string if_id = main_sensor ? "Laser" : ("Laser " + __cfg_name);
00076 
00077   if (__num_values == 360) {
00078     __laser360_if = blackboard->open_for_writing<Laser360Interface>(if_id.c_str());
00079     __laser360_if->set_frame(__cfg_frame.c_str());
00080     __laser360_if->write();
00081   } else if (__num_values == 720){
00082     __laser720_if = blackboard->open_for_writing<Laser720Interface>(if_id.c_str());
00083     __laser720_if->set_frame(__cfg_frame.c_str());
00084     __laser720_if->write();
00085   } else {
00086     throw Exception("Laser acquisition thread must produce either 360 or 720 "
00087                     "distance values, but it produces %u", __aqt->get_distance_data_size());
00088   }
00089 
00090 }
00091 
00092 
00093 void
00094 LaserSensorThread::finalize()
00095 {
00096   blackboard->close(__laser360_if);
00097   blackboard->close(__laser720_if);
00098 }
00099 
00100 void
00101 LaserSensorThread::loop()
00102 {
00103   if ( __aqt->lock_if_new_data() ) {
00104     if (__num_values == 360) {
00105       __laser360_if->set_distances(__aqt->get_distance_data());
00106       __laser360_if->write();
00107     } else if (__num_values == 720) {
00108       __laser720_if->set_distances(__aqt->get_distance_data());
00109        __laser720_if->write();
00110     }
00111     __aqt->unlock();
00112   }
00113 }