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
00030 #include <interfaces/Laser360Interface.h>
00031 #include <interfaces/Laser720Interface.h>
00032
00033 using namespace fawkes;
00034 
00035 /** @class LaserSensorThread "sensor_thread.h"
00036  * Laser sensor thread.
00037  * This thread integrates into the Fawkes main loop at the sensor hook and
00038  * publishes new data when available from the LaserAcquisitionThread.
00039  * @author Tim Niemueller
00040  */
00041
00042 
00043 /** Constructor.
00044  * @param aqt LaserAcquisitionThread to get data from
00045  */
00046 LaserSensorThread::LaserSensorThread(LaserAcquisitionThread *aqt)
00047   : Thread("LaserSensorThread", Thread::OPMODE_WAITFORWAKEUP),
00048     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR)
00049 {
00050   __aqt    = aqt;
00051 }
00052
00053
00054 void
00055 LaserSensorThread::init()
00056 {
00057   __laser360_if = NULL;
00058   __laser720_if = NULL;
00059
00060   bool spots_filter = false;
00061   try {
00062     spots_filter = config->get_bool("/hardware/laser/use_dead_spots_filter");
00063   } catch (Exception &e) {
00064   }
00065
00066   __aqt->pre_init(config, logger);
00067
00068   __num_values = __aqt->get_distance_data_size();
00069
00070   __filters360 = new LaserDataFilterCascade();
00071   __filters720 = new LaserDataFilterCascade();
00072
00073   if (__num_values == 360) {
00074     __laser360_if = blackboard->open_for_writing<Laser360Interface>("Laser");
00075   } else if (__num_values == 720){
00076     __laser360_if = blackboard->open_for_writing<Laser360Interface>("Laser");
00077     __laser720_if = blackboard->open_for_writing<Laser720Interface>("Laser");
00078     __filters360->add_filter(new Laser720to360DataFilter());
00079   } else {
00080     throw Exception("Laser acquisition thread must produce either 360 or 720 "
00081                     "distance values, but it produces %u", __aqt->get_distance_data_size());
00082   }
00083
00084   if (spots_filter) {
00085     std::string spots_prefix = "/hardware/laser/dead_spots/";
00086     logger->log_debug(name(), "Setting up dead spots filter for 360° interface");
00087     __filters360->add_filter(new LaserDeadSpotsDataFilter(config, logger, spots_prefix));
00088     logger->log_debug(name(), "Setting up dead spots filter for 720° interface");
00089     __filters720->add_filter(new LaserDeadSpotsDataFilter(config, logger, spots_prefix));
00090   }
00091 }
00092
00093
00094 void
00095 LaserSensorThread::finalize()
00096 {
00097   delete __filters360;
00098   delete __filters720;
00099   blackboard->close(__laser360_if);
00100   blackboard->close(__laser720_if);
00101 }
00102
00103 void
00104 LaserSensorThread::loop()
00105 {
00106   if ( __aqt->lock_if_new_data() ) {
00107     if (__num_values == 360) {
00108       if (__filters360->has_filters()) {
00109         __filters360->filter(__aqt->get_distance_data(), __aqt->get_distance_data_size());
00110         __laser360_if->set_distances(__filters360->filtered_data());
00111       } else {
00112         __laser360_if->set_distances(__aqt->get_distance_data());
00113       }
00114     } else if (__num_values == 720) {
00115       if (__filters720->has_filters()) {
00116         __filters720->filter(__aqt->get_distance_data(), __aqt->get_distance_data_size());
00117         __laser720_if->set_distances(__filters720->filtered_data());
00118       } else {
00119         __laser720_if->set_distances(__aqt->get_distance_data());
00120       }
00121       __filters360->filter(__aqt->get_distance_data(), __aqt->get_distance_data_size());
00122       __laser360_if->set_distances(__filters360->filtered_data());
00123     }
00124     __laser360_if->write();
00125     if (__laser720_if)  __laser720_if->write();
00126     __aqt->unlock();
00127   }
00128 }