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 #include "filters/circle.h"
00026 #include "filters/720to360.h"
00027 #include "filters/deadspots.h"
00028 #include "filters/cascade.h"
00029 #include "filters/reverse_angle.h"
00030 
00031 #include <interfaces/Laser360Interface.h>
00032 #include <interfaces/Laser720Interface.h>
00033 
00034 using namespace fawkes;
00035 
00036 /** @class LaserSensorThread "sensor_thread.h"
00037  * Laser sensor thread.
00038  * This thread integrates into the Fawkes main loop at the sensor hook and
00039  * publishes new data when available from the LaserAcquisitionThread.
00040  * @author Tim Niemueller
00041  */
00042 
00043 
00044 /** Constructor.
00045  * @param cfg_name short name of configuration group
00046  * @param cfg_prefix configuration path prefix
00047  * @param aqt LaserAcquisitionThread to get data from
00048  */
00049 LaserSensorThread::LaserSensorThread(std::string &cfg_name,
00050                                      std::string &cfg_prefix,
00051                                      LaserAcquisitionThread *aqt)
00052   : Thread("LaserSensorThread", Thread::OPMODE_WAITFORWAKEUP),
00053     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR)
00054 {
00055   set_name("LaserSensorThread(%s)", cfg_name.c_str());
00056   __aqt        = aqt;
00057   __cfg_name   = cfg_name;
00058   __cfg_prefix = cfg_prefix;
00059 }
00060 
00061 
00062 void
00063 LaserSensorThread::init()
00064 {
00065   __laser360_if = NULL;
00066   __laser720_if = NULL;
00067 
00068   bool spots_filter = false;
00069   bool main_sensor  = false;
00070   __clockwise_angle = false;
00071 
00072   try {
00073     spots_filter = config->get_bool((__cfg_prefix + "use_dead_spots_filter").c_str());
00074   } catch (Exception &e) {} // ignored, assume no
00075   try {
00076     main_sensor = config->get_bool((__cfg_prefix + "main_sensor").c_str());
00077   } catch (Exception &e) {} // ignored, assume no
00078   try {
00079     __clockwise_angle = config->get_bool((__cfg_prefix + "clockwise_angle").c_str());
00080   } catch (Exception &e) {} // ignored, assume no
00081 
00082   __aqt->pre_init(config, logger);
00083 
00084   __num_values = __aqt->get_distance_data_size();
00085 
00086   __filters360 = new LaserDataFilterCascade();
00087   __filters720 = new LaserDataFilterCascade();
00088 
00089   std::string if_id = main_sensor ? "Laser" : ("Laser " + __cfg_name);
00090 
00091   if (__num_values == 360) {
00092     __laser360_if = blackboard->open_for_writing<Laser360Interface>(if_id.c_str());
00093   } else if (__num_values == 720){
00094     __laser360_if = blackboard->open_for_writing<Laser360Interface>(if_id.c_str());
00095     __laser720_if = blackboard->open_for_writing<Laser720Interface>(if_id.c_str());
00096     __filters360->add_filter(new Laser720to360DataFilter());
00097   } else {
00098     throw Exception("Laser acquisition thread must produce either 360 or 720 "
00099                     "distance values, but it produces %u", __aqt->get_distance_data_size());
00100   }
00101 
00102   if (__clockwise_angle) {
00103     logger->log_debug(name(), "Setting up reverse angle filter for 360° interface");
00104     std::string rev_id = if_id + " CW";
00105     try {
00106       __reverse360_if = blackboard->open_for_writing<Laser360Interface>(rev_id.c_str());
00107       __reverse360_if->set_clockwise_angle(true);
00108       __reverse360_if->write();
00109       __reverse360 = new LaserReverseAngleDataFilter(360);
00110     } catch (Exception &e) {
00111       blackboard->close(__laser360_if);
00112       blackboard->close(__laser720_if);
00113       throw;
00114     }
00115 
00116     if (__num_values == 720) {
00117       logger->log_debug(name(), "Setting up dead spots filter for 720° interface");
00118       try {
00119         __reverse720_if = blackboard->open_for_writing<Laser720Interface>(rev_id.c_str());
00120         __reverse720_if->set_clockwise_angle(true);
00121         __reverse720_if->write();
00122         __reverse720 = new LaserReverseAngleDataFilter(720);
00123       } catch (Exception &e) {
00124         blackboard->close(__laser360_if);
00125         blackboard->close(__laser720_if);
00126         blackboard->close(__reverse360_if);
00127         delete __reverse360;
00128         throw;
00129       }
00130     }
00131   }
00132 
00133   if (spots_filter) {
00134     std::string spots_prefix = __cfg_prefix + "dead_spots/";
00135     logger->log_debug(name(), "Setting up dead spots filter for 360° interface");
00136     __filters360->add_filter(new LaserDeadSpotsDataFilter(config, logger, spots_prefix));
00137     logger->log_debug(name(), "Setting up dead spots filter for 720° interface");
00138     __filters720->add_filter(new LaserDeadSpotsDataFilter(config, logger, spots_prefix));
00139   }
00140 
00141 }
00142 
00143 
00144 void
00145 LaserSensorThread::finalize()
00146 {
00147   delete __filters360;
00148   delete __filters720;
00149   blackboard->close(__laser360_if);
00150   blackboard->close(__laser720_if);
00151 }
00152 
00153 void
00154 LaserSensorThread::loop()
00155 {
00156   if ( __aqt->lock_if_new_data() ) {
00157     if (__num_values == 360) {
00158       if (__filters360->has_filters()) {
00159         __filters360->filter(__aqt->get_distance_data(), __aqt->get_distance_data_size());
00160         __laser360_if->set_distances(__filters360->filtered_data());
00161       } else {
00162         __laser360_if->set_distances(__aqt->get_distance_data());
00163       }
00164 
00165       // We also provide the clockwise output
00166       if (__clockwise_angle) {
00167         __reverse360->filter(__laser360_if->distances(), 360);
00168         __reverse360_if->set_distances(__reverse360->filtered_data());
00169         __reverse360_if->write();
00170       }
00171     } else if (__num_values == 720) {
00172       if (__filters720->has_filters()) {
00173         __filters720->filter(__aqt->get_distance_data(), __aqt->get_distance_data_size());
00174         __laser720_if->set_distances(__filters720->filtered_data());
00175       } else {
00176         __laser720_if->set_distances(__aqt->get_distance_data());
00177       }
00178       __filters360->filter(__aqt->get_distance_data(), __aqt->get_distance_data_size());
00179       __laser360_if->set_distances(__filters360->filtered_data());
00180 
00181       // We also provide the clockwise output
00182       if (__clockwise_angle) {
00183         __reverse360->filter(__laser360_if->distances(), 360);
00184         __reverse360_if->set_distances(__reverse360->filtered_data());
00185         __reverse360_if->write();
00186         __reverse720->filter(__laser720_if->distances(), 720);
00187         __reverse720_if->set_distances(__reverse720->filtered_data());
00188         __reverse720_if->write();
00189       }
00190     }
00191     __laser360_if->write();
00192     if (__laser720_if)  __laser720_if->write();
00193     __aqt->unlock();
00194   }
00195 }

Generated on Tue Feb 22 13:31:28 2011 for Fawkes API by  doxygen 1.4.7