qa_objpos_majority.cpp

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

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