00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
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
00046
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",
00072 "WM Ball *",
00073 output->id(),
00074 1.0);
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);
00112 own->set_distance(1.5);
00113 own->set_relative_x(1.7);
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",
00142 "WM Ball *",
00143 output->id(),
00144 1.0);
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);
00183 own->set_distance(1.5);
00184 own->set_relative_x(1.7);
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",
00210 "WM Ball *",
00211 output->id(),
00212 1.0);
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);
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