Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * qa_bb_interface.h - BlackBoard interface QA 00004 * 00005 * Generated: Tue Oct 17 15:48:45 2006 00006 * Copyright 2006 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. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 /// @cond QA 00025 00026 #include <blackboard/local.h> 00027 #include <blackboard/exceptions.h> 00028 #include <blackboard/bbconfig.h> 00029 00030 #include <interfaces/ObjectPositionInterface.h> 00031 00032 #include <core/exceptions/system.h> 00033 #include <utils/logging/liblogger.h> 00034 #include <utils/logging/logger.h> 00035 #include <utils/logging/console.h> 00036 00037 #include <signal.h> 00038 #include <cstdlib> 00039 00040 #include <iostream> 00041 #include <sstream> 00042 #include <string> 00043 #include <vector> 00044 00045 // I know this isn't very beautiful :-) 00046 // Is there a library for the worldmodel I could link with? 00047 #include "../fuser.cpp" 00048 #include "../objpos_majority.cpp" 00049 00050 using namespace std; 00051 using namespace fawkes; 00052 00053 00054 bool quit = false; 00055 00056 void 00057 signal_handler(int signum) 00058 { 00059 quit = true; 00060 } 00061 00062 00063 typedef ObjectPositionInterface Opi; 00064 00065 void 00066 test1(Logger* logger, BlackBoard* bb, 00067 Opi* own, const vector<Opi*>& foreign, Opi* output) 00068 { 00069 cout << endl << endl << "Test #1:" << endl; 00070 WorldModelObjPosMajorityFuser fuser(logger, bb, 00071 "OmniBall", // own_id 00072 "WM Ball *", // foreign_id_pattern 00073 output->id(), 00074 1.0); // self-confidence 00075 00076 float x = 0.0; 00077 float y = 0.0; 00078 int i = 1; 00079 for (vector<Opi*>::const_iterator it = foreign.begin(); 00080 it != foreign.end(); ++it) { 00081 Opi* opi = *it; 00082 opi->set_valid(true); 00083 opi->set_flags(Opi::FLAG_HAS_WORLD); 00084 opi->set_visible(true); 00085 opi->set_world_x(x); 00086 opi->set_world_y(y); 00087 opi->set_world_z(0.0); 00088 opi->write(); 00089 x += 0.1f; 00090 y += 0.1f; 00091 if (i == 2) { 00092 x += 3.0f; 00093 y += 3.0f; 00094 } else if (i == 5) { 00095 x += 5.0f; 00096 y += 5.0f; 00097 } 00098 cout << " Set foreign " << opi->id() << " world " << 00099 "(" << opi->world_x() << ", " << opi->world_y() << ")" << endl; 00100 ++i; 00101 } 00102 00103 own->set_valid(true); 00104 own->set_flags(Opi::FLAG_HAS_WORLD 00105 | Opi::FLAG_HAS_RELATIVE_CARTESIAN 00106 | Opi::FLAG_HAS_RELATIVE_POLAR); 00107 own->set_visible(true); 00108 own->set_world_x(2.0); 00109 own->set_world_y(2.0); 00110 own->set_world_z(0.0); 00111 own->set_bearing(0.5); // doesn't matter 00112 own->set_distance(1.5); // set to 0.5 to enforce copy-of-own 00113 own->set_relative_x(1.7); // set to 0.7 to enforce copy-of-own 00114 own->set_relative_y(0.7); 00115 own->write(); 00116 cout << " Set own " << own->id() << " world " << 00117 "(" << own->world_x() << ", " << own->world_y() << ")" << endl; 00118 00119 fuser.fuse(); 00120 00121 cout << " Reading output interface.. " << flush; 00122 output->read(); 00123 cout << "(" << output->world_x() << ", " << output->world_y() << ")"; 00124 cout << endl; 00125 00126 float expected_x = (3.2f + 3.3f + 3.4f) / 3; 00127 float expected_y = (3.2f + 3.3f + 3.4f) / 3; 00128 if (output->world_x() != expected_x || 00129 output->world_y() != expected_y) { 00130 throw Exception("Should have averaged interfaces 2, 3, 4 "\ 00131 "(starting from 0)."); 00132 } 00133 } 00134 00135 void 00136 test2(Logger* logger, BlackBoard* bb, 00137 Opi* own, const vector<Opi*>& foreign, Opi* output) 00138 { 00139 cout << endl << endl << "Test #2:" << endl; 00140 WorldModelObjPosMajorityFuser fuser(logger, bb, 00141 "OmniBall", // own_id 00142 "WM Ball *", // foreign_id_pattern 00143 output->id(), 00144 1.0); // self-confidence 00145 00146 float x = 0.0; 00147 float y = 0.0; 00148 int i = 1; 00149 for (vector<Opi*>::const_iterator it = foreign.begin(); 00150 it != foreign.end(); ++it) { 00151 Opi* opi = *it; 00152 opi->set_valid(true); 00153 opi->set_flags(Opi::FLAG_HAS_WORLD); 00154 opi->set_visible(true); 00155 opi->set_world_x(x); 00156 opi->set_world_y(y); 00157 opi->set_world_z(0.0); 00158 opi->write(); 00159 x += 0.1f; 00160 y += 0.1f; 00161 if (i == 2) { 00162 x += 3.0f; 00163 y += 3.0f; 00164 } else if (i == 4) { 00165 x += 5.0f; 00166 y += 5.0f; 00167 } 00168 cout << " Set foreign " << opi->id() << " world " << 00169 "(" << opi->world_x() << ", " << opi->world_y() << ")" << endl; 00170 ++i; 00171 } 00172 00173 own->set_valid(true); 00174 own->set_flags(Opi::FLAG_HAS_WORLD 00175 | Opi::FLAG_HAS_RELATIVE_CARTESIAN 00176 | Opi::FLAG_HAS_RELATIVE_POLAR 00177 ); 00178 own->set_visible(true); 00179 own->set_world_x(2.0); 00180 own->set_world_y(2.0); 00181 own->set_world_z(0.0); 00182 own->set_bearing(0.5); // doesn't matter 00183 own->set_distance(1.5); // set to 0.5 to enforce copy-of-own 00184 own->set_relative_x(1.7); // set to 0.7 to enforce copy-of-own 00185 own->set_relative_y(0.7); 00186 own->write(); 00187 cout << " Set own " << own->id() << " world " << 00188 "(" << own->world_x() << ", " << own->world_y() << ")" << endl; 00189 00190 fuser.fuse(); 00191 00192 cout << " Reading output interface.. " << flush; 00193 output->read(); 00194 cout << "(" << output->world_x() << ", " << output->world_y() << ")"; 00195 cout << endl; 00196 00197 if (output->world_x() != own->world_x() || 00198 output->world_y() != own->world_y()) { 00199 throw Exception("Should have copied own interface because "\ 00200 "no majority was found."); 00201 } 00202 } 00203 void 00204 test3(Logger* logger, BlackBoard* bb, 00205 Opi* own, const vector<Opi*>& foreign, Opi* output) 00206 { 00207 cout << endl << endl << "Test #3:" << endl; 00208 WorldModelObjPosMajorityFuser fuser(logger, bb, 00209 "OmniBall", // own_id 00210 "WM Ball *", // foreign_id_pattern 00211 output->id(), 00212 1.0); // self-confidence 00213 00214 float x = 0.0; 00215 float y = 0.0; 00216 int i = 1; 00217 for (vector<Opi*>::const_iterator it = foreign.begin(); 00218 it != foreign.end(); ++it) { 00219 Opi* opi = *it; 00220 opi->set_valid(true); 00221 opi->set_flags(Opi::FLAG_HAS_WORLD); 00222 opi->set_visible(true); 00223 opi->set_world_x(x); 00224 opi->set_world_y(y); 00225 opi->set_world_z(0.0); 00226 opi->write(); 00227 x += 0.1f; 00228 y += 0.1f; 00229 if (i == 2) { 00230 x += 3.0f; 00231 y += 3.0f; 00232 } else if (i == 4) { 00233 x += 5.0f; 00234 y += 5.0f; 00235 } 00236 cout << " Set foreign " << opi->id() << " world " << 00237 "(" << opi->world_x() << ", " << opi->world_y() << ")" << endl; 00238 ++i; 00239 } 00240 00241 own->set_valid(true); 00242 own->set_flags(Opi::FLAG_HAS_WORLD 00243 | Opi::FLAG_HAS_RELATIVE_CARTESIAN 00244 | Opi::FLAG_HAS_RELATIVE_POLAR 00245 ); 00246 own->set_visible(true); 00247 own->set_world_x(2.0); 00248 own->set_world_y(2.0); 00249 own->set_world_z(0.0); 00250 own->set_bearing(0.5); // doesn't matter 00251 own->set_distance(0.5); 00252 own->set_relative_x(0.7); 00253 own->set_relative_y(0.7); 00254 own->write(); 00255 cout << " Set own " << own->id() << " world " << 00256 "(" << own->world_x() << ", " << own->world_y() << ")" << endl; 00257 00258 fuser.fuse(); 00259 00260 cout << " Reading output interface.. " << flush; 00261 output->read(); 00262 cout << "(" << output->world_x() << ", " << output->world_y() << ")"; 00263 cout << endl; 00264 00265 if (output->world_x() != own->world_x() || 00266 output->world_y() != own->world_y()) { 00267 throw Exception("Should have copied own interface because "\ 00268 "it's near enough"); 00269 } 00270 } 00271 00272 int 00273 main(int argc, char **argv) 00274 { 00275 LibLogger::init(); 00276 Logger* logger = new ConsoleLogger(); 00277 00278 signal(SIGINT, signal_handler); 00279 00280 LocalBlackBoard *lbb = new LocalBlackBoard(BLACKBOARD_MEMSIZE); 00281 00282 BlackBoard* bb = lbb; 00283 00284 00285 Opi* own; 00286 vector<Opi*> foreign; 00287 Opi* output; 00288 00289 try { 00290 cout << "Opening interfaces.. " << flush; 00291 own = bb->open_for_writing<Opi>("OmniBall"); 00292 for (int i = 1; i <= 5; i++) { 00293 stringstream stream; 00294 stream << "WM Ball "; 00295 stream << i; 00296 string id = stream.str(); 00297 Opi* opi = bb->open_for_writing<Opi>(id.c_str()); 00298 foreign.push_back(opi); 00299 } 00300 output = bb->open_for_reading<Opi>("WM Ball"); 00301 cout << "done" << endl; 00302 } catch (Exception &e) { 00303 cout << "failed! Aborting" << endl; 00304 e.print_trace(); 00305 exit(1); 00306 } 00307 00308 test1(logger, bb, own, foreign, output); 00309 test2(logger, bb, own, foreign, output); 00310 test3(logger, bb, own, foreign, output); 00311 00312 cout << "Closing interfaces.. " << flush; 00313 bb->close(own); 00314 for (vector<Opi*>::iterator it = foreign.begin(); 00315 it != foreign.end(); ++it) { 00316 Opi* opi = *it; 00317 bb->close(opi); 00318 } 00319 cout << "done" << endl; 00320 00321 cout << "Deleting blackboard.. " << flush; 00322 delete bb; 00323 cout << "done" << endl; 00324 cout << "Finalizing logger.. " << flush; 00325 LibLogger::finalize(); 00326 cout << "done" << endl; 00327 } 00328 00329 00330 /// @endcond