Fawkes API
Fawkes Development Version
|
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 }