00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
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) {}
00075 try {
00076 main_sensor = config->get_bool((__cfg_prefix + "main_sensor").c_str());
00077 } catch (Exception &e) {}
00078 try {
00079 __clockwise_angle = config->get_bool((__cfg_prefix + "clockwise_angle").c_str());
00080 } catch (Exception &e) {}
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
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
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 }