Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
qa_objpos_majority.cpp
1 
2 /***************************************************************************
3  * qa_bb_interface.h - BlackBoard interface QA
4  *
5  * Generated: Tue Oct 17 15:48:45 2006
6  * Copyright 2006 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 /// @cond QA
25 
26 #include <blackboard/local.h>
27 #include <blackboard/exceptions.h>
28 #include <blackboard/bbconfig.h>
29 
30 #include <interfaces/ObjectPositionInterface.h>
31 
32 #include <core/exceptions/system.h>
33 #include <utils/logging/liblogger.h>
34 #include <utils/logging/logger.h>
35 #include <utils/logging/console.h>
36 
37 #include <signal.h>
38 #include <cstdlib>
39 
40 #include <iostream>
41 #include <sstream>
42 #include <string>
43 #include <vector>
44 
45 // I know this isn't very beautiful :-)
46 // Is there a library for the worldmodel I could link with?
47 #include "../fuser.cpp"
48 #include "../objpos_majority.cpp"
49 
50 using namespace std;
51 using namespace fawkes;
52 
53 
54 bool quit = false;
55 
56 void
57 signal_handler(int signum)
58 {
59  quit = true;
60 }
61 
62 
63 typedef ObjectPositionInterface Opi;
64 
65 void
66 test1(Logger* logger, BlackBoard* bb,
67  Opi* own, const vector<Opi*>& foreign, Opi* output)
68 {
69  cout << endl << endl << "Test #1:" << endl;
70  WorldModelObjPosMajorityFuser fuser(logger, bb,
71  "OmniBall", // own_id
72  "WM Ball *", // foreign_id_pattern
73  output->id(),
74  1.0); // self-confidence
75 
76  float x = 0.0;
77  float y = 0.0;
78  int i = 1;
79  for (vector<Opi*>::const_iterator it = foreign.begin();
80  it != foreign.end(); ++it) {
81  Opi* opi = *it;
82  opi->set_valid(true);
83  opi->set_flags(Opi::FLAG_HAS_WORLD);
84  opi->set_visible(true);
85  opi->set_world_x(x);
86  opi->set_world_y(y);
87  opi->set_world_z(0.0);
88  opi->write();
89  x += 0.1f;
90  y += 0.1f;
91  if (i == 2) {
92  x += 3.0f;
93  y += 3.0f;
94  } else if (i == 5) {
95  x += 5.0f;
96  y += 5.0f;
97  }
98  cout << " Set foreign " << opi->id() << " world " <<
99  "(" << opi->world_x() << ", " << opi->world_y() << ")" << endl;
100  ++i;
101  }
102 
103  own->set_valid(true);
104  own->set_flags(Opi::FLAG_HAS_WORLD
105  | Opi::FLAG_HAS_RELATIVE_CARTESIAN
106  | Opi::FLAG_HAS_RELATIVE_POLAR);
107  own->set_visible(true);
108  own->set_world_x(2.0);
109  own->set_world_y(2.0);
110  own->set_world_z(0.0);
111  own->set_bearing(0.5); // doesn't matter
112  own->set_distance(1.5); // set to 0.5 to enforce copy-of-own
113  own->set_relative_x(1.7); // set to 0.7 to enforce copy-of-own
114  own->set_relative_y(0.7);
115  own->write();
116  cout << " Set own " << own->id() << " world " <<
117  "(" << own->world_x() << ", " << own->world_y() << ")" << endl;
118 
119  fuser.fuse();
120 
121  cout << " Reading output interface.. " << flush;
122  output->read();
123  cout << "(" << output->world_x() << ", " << output->world_y() << ")";
124  cout << endl;
125 
126  float expected_x = (3.2f + 3.3f + 3.4f) / 3;
127  float expected_y = (3.2f + 3.3f + 3.4f) / 3;
128  if (output->world_x() != expected_x ||
129  output->world_y() != expected_y) {
130  throw Exception("Should have averaged interfaces 2, 3, 4 "\
131  "(starting from 0).");
132  }
133 }
134 
135 void
136 test2(Logger* logger, BlackBoard* bb,
137  Opi* own, const vector<Opi*>& foreign, Opi* output)
138 {
139  cout << endl << endl << "Test #2:" << endl;
140  WorldModelObjPosMajorityFuser fuser(logger, bb,
141  "OmniBall", // own_id
142  "WM Ball *", // foreign_id_pattern
143  output->id(),
144  1.0); // self-confidence
145 
146  float x = 0.0;
147  float y = 0.0;
148  int i = 1;
149  for (vector<Opi*>::const_iterator it = foreign.begin();
150  it != foreign.end(); ++it) {
151  Opi* opi = *it;
152  opi->set_valid(true);
153  opi->set_flags(Opi::FLAG_HAS_WORLD);
154  opi->set_visible(true);
155  opi->set_world_x(x);
156  opi->set_world_y(y);
157  opi->set_world_z(0.0);
158  opi->write();
159  x += 0.1f;
160  y += 0.1f;
161  if (i == 2) {
162  x += 3.0f;
163  y += 3.0f;
164  } else if (i == 4) {
165  x += 5.0f;
166  y += 5.0f;
167  }
168  cout << " Set foreign " << opi->id() << " world " <<
169  "(" << opi->world_x() << ", " << opi->world_y() << ")" << endl;
170  ++i;
171  }
172 
173  own->set_valid(true);
174  own->set_flags(Opi::FLAG_HAS_WORLD
175  | Opi::FLAG_HAS_RELATIVE_CARTESIAN
176  | Opi::FLAG_HAS_RELATIVE_POLAR
177  );
178  own->set_visible(true);
179  own->set_world_x(2.0);
180  own->set_world_y(2.0);
181  own->set_world_z(0.0);
182  own->set_bearing(0.5); // doesn't matter
183  own->set_distance(1.5); // set to 0.5 to enforce copy-of-own
184  own->set_relative_x(1.7); // set to 0.7 to enforce copy-of-own
185  own->set_relative_y(0.7);
186  own->write();
187  cout << " Set own " << own->id() << " world " <<
188  "(" << own->world_x() << ", " << own->world_y() << ")" << endl;
189 
190  fuser.fuse();
191 
192  cout << " Reading output interface.. " << flush;
193  output->read();
194  cout << "(" << output->world_x() << ", " << output->world_y() << ")";
195  cout << endl;
196 
197  if (output->world_x() != own->world_x() ||
198  output->world_y() != own->world_y()) {
199  throw Exception("Should have copied own interface because "\
200  "no majority was found.");
201  }
202 }
203 void
204 test3(Logger* logger, BlackBoard* bb,
205  Opi* own, const vector<Opi*>& foreign, Opi* output)
206 {
207  cout << endl << endl << "Test #3:" << endl;
208  WorldModelObjPosMajorityFuser fuser(logger, bb,
209  "OmniBall", // own_id
210  "WM Ball *", // foreign_id_pattern
211  output->id(),
212  1.0); // self-confidence
213 
214  float x = 0.0;
215  float y = 0.0;
216  int i = 1;
217  for (vector<Opi*>::const_iterator it = foreign.begin();
218  it != foreign.end(); ++it) {
219  Opi* opi = *it;
220  opi->set_valid(true);
221  opi->set_flags(Opi::FLAG_HAS_WORLD);
222  opi->set_visible(true);
223  opi->set_world_x(x);
224  opi->set_world_y(y);
225  opi->set_world_z(0.0);
226  opi->write();
227  x += 0.1f;
228  y += 0.1f;
229  if (i == 2) {
230  x += 3.0f;
231  y += 3.0f;
232  } else if (i == 4) {
233  x += 5.0f;
234  y += 5.0f;
235  }
236  cout << " Set foreign " << opi->id() << " world " <<
237  "(" << opi->world_x() << ", " << opi->world_y() << ")" << endl;
238  ++i;
239  }
240 
241  own->set_valid(true);
242  own->set_flags(Opi::FLAG_HAS_WORLD
243  | Opi::FLAG_HAS_RELATIVE_CARTESIAN
244  | Opi::FLAG_HAS_RELATIVE_POLAR
245  );
246  own->set_visible(true);
247  own->set_world_x(2.0);
248  own->set_world_y(2.0);
249  own->set_world_z(0.0);
250  own->set_bearing(0.5); // doesn't matter
251  own->set_distance(0.5);
252  own->set_relative_x(0.7);
253  own->set_relative_y(0.7);
254  own->write();
255  cout << " Set own " << own->id() << " world " <<
256  "(" << own->world_x() << ", " << own->world_y() << ")" << endl;
257 
258  fuser.fuse();
259 
260  cout << " Reading output interface.. " << flush;
261  output->read();
262  cout << "(" << output->world_x() << ", " << output->world_y() << ")";
263  cout << endl;
264 
265  if (output->world_x() != own->world_x() ||
266  output->world_y() != own->world_y()) {
267  throw Exception("Should have copied own interface because "\
268  "it's near enough");
269  }
270 }
271 
272 int
273 main(int argc, char **argv)
274 {
275  LibLogger::init();
276  Logger* logger = new ConsoleLogger();
277 
278  signal(SIGINT, signal_handler);
279 
280  LocalBlackBoard *lbb = new LocalBlackBoard(BLACKBOARD_MEMSIZE);
281 
282  BlackBoard* bb = lbb;
283 
284 
285  Opi* own;
286  vector<Opi*> foreign;
287  Opi* output;
288 
289  try {
290  cout << "Opening interfaces.. " << flush;
291  own = bb->open_for_writing<Opi>("OmniBall");
292  for (int i = 1; i <= 5; i++) {
293  stringstream stream;
294  stream << "WM Ball ";
295  stream << i;
296  string id = stream.str();
297  Opi* opi = bb->open_for_writing<Opi>(id.c_str());
298  foreign.push_back(opi);
299  }
300  output = bb->open_for_reading<Opi>("WM Ball");
301  cout << "done" << endl;
302  } catch (Exception &e) {
303  cout << "failed! Aborting" << endl;
304  e.print_trace();
305  exit(1);
306  }
307 
308  test1(logger, bb, own, foreign, output);
309  test2(logger, bb, own, foreign, output);
310  test3(logger, bb, own, foreign, output);
311 
312  cout << "Closing interfaces.. " << flush;
313  bb->close(own);
314  for (vector<Opi*>::iterator it = foreign.begin();
315  it != foreign.end(); ++it) {
316  Opi* opi = *it;
317  bb->close(opi);
318  }
319  cout << "done" << endl;
320 
321  cout << "Deleting blackboard.. " << flush;
322  delete bb;
323  cout << "done" << endl;
324  cout << "Finalizing logger.. " << flush;
325  LibLogger::finalize();
326  cout << "done" << endl;
327 }
328 
329 
330 /// @endcond
ObjectPositionInterface Fawkes BlackBoard Interface.
Interface for logging to stderr.
Definition: console.h:35
Local BlackBoard.
Definition: local.h:45
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
ObjectPositionInterface majority fuser.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
The BlackBoard abstract class.
Definition: blackboard.h:49
virtual void close(Interface *interface)=0
Close interface.
Interface for logging.
Definition: logger.h:34