Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
objpos_majority.cpp
1 
2 /***************************************************************************
3  * objpos_majority.cpp - Fawkes WorldModel Object Position Majority Fuser
4  *
5  * Created: Thu 01 Apr 2010 05:06:36 PM CEST
6  * Copyright 2010 Christoph Schwering
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.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "objpos_majority.h"
24 
25 #include <iostream>
26 #include <cmath>
27 #include <cstring>
28 #include <list>
29 
30 #include <core/threading/mutex_locker.h>
31 #include <blackboard/blackboard.h>
32 #include <logging/logger.h>
33 
34 /** @class WorldModelObjPosMajorityFuser "objpos_majority.h"
35  * ObjectPositionInterface majority fuser.
36  * The parameters are (1) the ID of the own ObjectPositionInterface, (2) the
37  * pattern ID of the other robots' ObjectPositionInterfaces and (3) the
38  * maximum-self-confidence-distance.
39  *
40  * (1) If the own ObjectPositionInterface thinks the object is not further away
41  * than self_confidence_radius, then the own interface's data is copied to the
42  * output interface.
43  * (2) If there is an unambiguous majority of interfaces that say the object is
44  * somewhere else and this majority is averaged and the average values are
45  * copied to the output interface.
46  * Since the other interfaces probably won't agree on one exact position, they
47  * are grouped: for each interface A its group is the set of interfaces that
48  * claim the object is not further away from the position claimed by A than
49  * GROUP_RADIUS. GROUP_RADIUS is currently hard-coded to 1.0 meters.
50  * (3) If the other interfaces "cannot settle" on some position of the object,
51  * the own interface's data is considered as at least as reliable as theirs and
52  * therefore the own interface's data is copied to the output interface.
53  *
54  * Like the WorldModelObjPosMajorityFuser, it registers as an observer and opens
55  * any newly created interface that matches the ID of the own
56  * ObjectPositionInterface or the pattern of the foreign
57  * ObjectPositionInterfaces.
58  * @author Christoph Schwering
59  */
60 
61 /** Constructor.
62  * @param blackboard BlackBoard.
63  * @param logger Logger.
64  * @param own_id The ID of the (single) own interface.
65  * @param foreign_id_pattern The pattern of the (multiple) other interfaces.
66  * @param output_id The ID of the destination interface.
67  * @param self_confidence_radius radius in which to consider our perception
68  * the best
69  */
71  fawkes::Logger* logger,
72  fawkes::BlackBoard* blackboard,
73  const std::string& own_id,
74  const std::string& foreign_id_pattern,
75  const std::string& output_id,
76  float self_confidence_radius)
77  : logger_(logger),
78  blackboard_(blackboard),
79  own_id_(own_id),
80  output_id_(output_id),
81  self_confidence_radius_(self_confidence_radius)
82 {
83  input_ifs_.clear();
84  output_if_ = NULL;
85  try {
86  own_if_ = blackboard_->open_for_reading<Opi>(own_id.c_str());
87  std::list<Opi*> input_ifs = blackboard_->open_multiple_for_reading<Opi>(
88  foreign_id_pattern.c_str());
89  for (std::list<Opi*>::const_iterator it = input_ifs.begin();
90  it != input_ifs.end(); ++it) {
91  Opi* opi = *it;
92  std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert(opi);
93  if (!ret.second) {
94  blackboard->close(opi);
95  }
96  }
97  if (own_if_ != NULL) {
98  std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert(own_if_);
99  if (!ret.second) {
100  blackboard->close(own_if_);
101  own_if_ = *ret.first;
102  }
103  }
104 
105  output_if_ = blackboard_->open_for_writing<Opi>(output_id.c_str());
106  OpiSet::iterator iter = input_ifs_.find(output_if_);
107  if (iter != input_ifs_.end()) {
108  Opi* opi = *iter;
109  blackboard->close(opi);
110  input_ifs_.erase(iter);
111  }
112  } catch (fawkes::Exception& e) {
113  for (OpiSet::const_iterator it = input_ifs_.begin();
114  it != input_ifs_.end(); ++it) {
115  blackboard->close(*it);
116  }
117  input_ifs_.clear();
118  if (output_if_ != NULL) {
119  blackboard->close(output_if_);
120  output_if_ = NULL;
121  }
122  throw;
123  }
124 
125  bbio_add_observed_create("ObjectPositionInterface",
126  own_id.c_str());
127  bbio_add_observed_create("ObjectPositionInterface",
128  foreign_id_pattern.c_str());
129  blackboard_->register_observer(this);
130 }
131 
132 
133 /** Destructor. */
135 {
136  blackboard_->unregister_observer(this);
137 
138  input_ifs_.lock();
139  for (OpiSet::const_iterator it = input_ifs_.begin();
140  it != input_ifs_.end(); ++it) {
141  blackboard_->close(*it);
142  }
143  input_ifs_.clear();
144  input_ifs_.unlock();
145 
146  if (output_if_ != NULL) {
147  blackboard_->close(output_if_);
148  }
149 }
150 
151 
152 void
154  const char *id) throw()
155 {
156  if (output_id_ == id) {
157  return;
158  }
159  Opi* from_if = NULL;
160  try {
161  from_if = blackboard_->open_for_reading<Opi>(id);
162  std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert_locked(from_if);
163  if (!ret.second) {
164  blackboard_->close(from_if);
165  }
166  Opi* inserted_if = *ret.first;
167  if (own_if_ == NULL && own_id_ == std::string(inserted_if->id())) {
168  own_if_ = inserted_if;
169  }
170  } catch (fawkes::Exception& e) {
171  if (from_if != NULL) {
172  blackboard_->close(from_if);
173  }
174  e.print_trace();
175  }
176 }
177 
178 float
179 WorldModelObjPosMajorityFuser::length(float x, float y, float z)
180 {
181  return sqrt(x*x + y*y + z*z);
182 }
183 
184 float
185 WorldModelObjPosMajorityFuser::rel_length(const Opi* opi)
186 {
187  return length(opi->relative_x(), opi->relative_y(), opi->relative_z());
188 }
189 
190 float
191 WorldModelObjPosMajorityFuser::world_object_dist(const Opi* from, const Opi* to)
192 {
193  return length(to->world_x() - from->world_x(),
194  to->world_y() - from->world_y(),
195  to->world_z() - from->world_z());
196 }
197 
198 bool
199 WorldModelObjPosMajorityFuser::same_contents(const OpiBucket& left,
200  const OpiBucket& right)
201 {
202  if (left.size() != right.size()) {
203  return false;
204  }
205 
206  std::set<OpiWrapper> rightSet(right.begin(), right.end());
207  for (OpiBucket::const_iterator it = left.begin();
208  it != left.end(); ++it) {
209  Opi* opi = *it;
210  if (rightSet.find(opi) == rightSet.end()) {
211  return false;
212  }
213  }
214  return true;
215 }
216 
217 void
219 {
220  if (own_if_ != NULL) {
221  own_if_->read();
222  }
223 
224  if (own_if_ != NULL &&
225  own_if_->has_writer() &&
226  own_if_->is_valid() &&
227  own_if_->is_visible() &&
228  (((own_if_->flags() & Opi::FLAG_HAS_RELATIVE_CARTESIAN) &&
229  rel_length(own_if_) <= self_confidence_radius_) ||
230  ((own_if_->flags() & Opi::FLAG_HAS_RELATIVE_POLAR) &&
231  own_if_->distance() <= self_confidence_radius_))) {
232  // Case 1: just copy own data if the own data claims the ball is very near.
233  copy_own_if();
234 
235  } else {
236  // Case 2: group interfaces, look for a majority and average it, if there is
237  // none, copy the own interface.
238 
239  for (OpiSet::const_iterator it = input_ifs_.begin();
240  it != input_ifs_.end(); ++it) {
241  Opi* opi = *it;
242  if (opi != own_if_) { // we've read own_if_ already
243  opi->read();
244  }
245  }
246  check();
247 
248  // Group interfaces in buckets.
249  input_ifs_.lock();
250  OpiBuckets buckets;
251  for (OpiSet::const_iterator it = input_ifs_.begin();
252  it != input_ifs_.end(); ++it) {
253  Opi* opi = *it;
254  if (!(opi->flags() & Opi::FLAG_HAS_WORLD) ||
255  !opi->has_writer() ||
256  !opi->is_valid() ||
257  !opi->is_visible()) {
258  continue;
259  }
260  OpiBucket bucket;
261  bucket.push_back(opi);
262  for (OpiSet::const_iterator jt = input_ifs_.begin();
263  jt != input_ifs_.end(); ++jt) {
264  Opi* candidate = *jt;
265  if (candidate != opi &&
266  (candidate->flags() & Opi::FLAG_HAS_WORLD) &&
267  world_object_dist(opi, candidate) <= GROUP_RADIUS) {
268  bucket.push_back(candidate);
269  }
270  }
271  buckets.push_back(bucket);
272  }
273  input_ifs_.unlock();
274 
275  // Search for majority.
276  OpiBucket majority;
277  bool unambiguous = false;
278  for (OpiBuckets::const_iterator it = buckets.begin();
279  it != buckets.end(); ++it) {
280  const OpiBucket& bucket = *it;
281  if (majority.size() <= bucket.size()) {
282  if (majority.size() < bucket.size()) {
283  majority = bucket;
284  unambiguous = true;
285  } else {
286  unambiguous = unambiguous && same_contents(majority, bucket);
287  }
288  }
289  }
290  if (majority.size() > 0 && unambiguous) {
291  // Case 2a: calculate average of majority.
292  average(majority);
293  } else {
294  // Case 2b: no majority found, copy own data.
295  copy_own_if();
296  }
297  }
298 }
299 
300 void
301 WorldModelObjPosMajorityFuser::check()
302 {
303  unsigned int base_flags = 0;
304 
305  unsigned int object_type = 0;
306  bool object_type_warned = false;
307  bool flags_read = false;
308 
309  for (OpiSet::const_iterator it = input_ifs_.begin();
310  it != input_ifs_.end(); ++it) {
311  Opi* opi = *it;
312  if (!opi->has_writer()) {
313  continue;
314  }
315  if (!opi->is_valid()) {
316  continue;
317  }
318  if (object_type != 0 && opi->object_type() != object_type &&
319  !object_type_warned) {
320  logger_->log_warn("WMObjPosAvgFus", "Object types of input interfaces "
321  "for %s disagree, %s has %u, expected was %u",
322  output_id_.c_str(), opi->uid(), opi->object_type(),
323  object_type);
324  object_type_warned = true;
325  } else {
326  object_type = opi->object_type();
327  }
328 
329  if (flags_read) {
330  unsigned int iflags = opi->flags()
331  & (0xFFFFFFFF ^ Opi::FLAG_HAS_WORLD)
332  & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_CARTESIAN)
333  & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_POLAR);
334  if (iflags != base_flags) {
335  logger_->log_warn("WMObjPosAvgFus", "Interface flags for %s "
336  "disagree. Exected %x, got %x", base_flags,
337  iflags);
338  }
339  } else {
340  base_flags = opi->flags()
341  & (0xFFFFFFFF ^ Opi::FLAG_HAS_WORLD)
342  & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_CARTESIAN)
343  & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_POLAR);
344  flags_read = true;
345  }
346  }
347 }
348 
349 void
350 WorldModelObjPosMajorityFuser::copy_own_if()
351 {
352  output_if_->copy_values(own_if_);
353  output_if_->write();
354 }
355 
356 /** Averages over the given input interfaces.
357  * The same like WorldModelObjPosAverageFuser::fuse() except that this method
358  * works on a parameter.
359  * (Making this fuser a subclass of the average fuser would make sense.)
360  * @param input_ifs The interface that are averaged.
361  */
362 void
363 WorldModelObjPosMajorityFuser::average(const OpiBucket& input_ifs)
364 {
365  unsigned int flags = 0;
366  unsigned int world_num_inputs = 0;
367  unsigned int extent_num_inputs = 0;
368  unsigned int euler_num_inputs = 0;
369  unsigned int worldvel_num_inputs = 0;
370  unsigned int relcart_num_inputs = 0;
371  unsigned int relpolar_num_inputs = 0;
372 
373  float roll = 0.0f;
374  float pitch = 0.0f;
375  float yaw = 0.0f;
376  float distance = 0.0f;
377  float bearing = 0.0f;
378  float slope = 0.0f;
379 
380  float world_x = 0.0f;
381  float world_y = 0.0f;
382  float world_z = 0.0f;
383 
384  float relative_x = 0.0f;
385  float relative_y = 0.0f;
386  float relative_z = 0.0f;
387 
388  float extent_x = 0.0f;
389  float extent_y = 0.0f;
390  float extent_z = 0.0f;
391 
392  float world_x_velocity = 0.0f;
393  float world_y_velocity = 0.0f;
394  float world_z_velocity = 0.0f;
395 
396  float relative_x_velocity = 0.0f;
397  float relative_y_velocity = 0.0f;
398  float relative_z_velocity = 0.0f;
399 
400  bool valid = true;
401  bool visible = true;
402  int vishistory_min = 0;
403  int vishistory_max = 0;
404  bool have_world = false, have_relative = false;
405 
406  for (OpiBucket::const_iterator it = input_ifs.begin();
407  it != input_ifs.end(); ++it) {
408  Opi* opi = *it;
409  if (!opi->has_writer()) {
410  continue;
411  }
412  opi->read();
413  if (!opi->is_valid()) {
414  continue;
415  }
416 
417  if (opi->is_visible()) {
418  if (opi->flags() & Opi::FLAG_HAS_WORLD) {
419  have_world = true;
420 
421  flags |= Opi::FLAG_HAS_WORLD;
422  world_x += opi->world_x();
423  world_y += opi->world_y();
424  world_z += opi->world_z();
425  world_num_inputs += 1;
426 
427  if (opi->flags() & Opi::FLAG_HAS_EULER_ANGLES) {
428  roll += opi->roll();
429  pitch += opi->pitch();
430  yaw += opi->yaw();
432  euler_num_inputs += 1;
433  }
434 
435  if (opi->flags() & Opi::FLAG_HAS_WORLD_VELOCITY) {
436  world_x_velocity += opi->world_x_velocity();
437  world_y_velocity += opi->world_y_velocity();
438  world_z_velocity += opi->world_z_velocity();
440  worldvel_num_inputs += 1;
441  }
442  }
443 
444  if (opi->flags() & Opi::FLAG_HAS_RELATIVE_CARTESIAN) {
445  have_relative = true;
446 
448 
449  relative_x += opi->relative_x();
450  relative_y += opi->relative_y();
451  relative_z += opi->relative_z();
452  relative_x_velocity += opi->relative_x_velocity();
453  relative_y_velocity += opi->relative_y_velocity();
454  relative_z_velocity += opi->relative_z_velocity();
455  relcart_num_inputs += 1;
456  }
457 
458  if (opi->flags() & Opi::FLAG_HAS_RELATIVE_POLAR) {
459  have_relative = true;
460 
462 
463  distance += opi->distance();
464  bearing += opi->bearing();
465  slope += opi->slope();
466  relpolar_num_inputs += 1;
467  }
468 
469  if (opi->flags() & Opi::FLAG_HAS_EXTENT) {
470  extent_x += opi->extent_x();
471  extent_y += opi->extent_y();
472  extent_z += opi->extent_z();
473  flags |= Opi::FLAG_HAS_EXTENT;
474  extent_num_inputs += 1;
475  }
476 
477  if (opi->visibility_history() > vishistory_max) {
478  vishistory_max = opi->visibility_history();
479  }
480  } else {
481  if (opi->visibility_history() < vishistory_min) {
482  vishistory_min = opi->visibility_history();
483  }
484  }
485  }
486 
487  if (world_num_inputs > 0) {
488  output_if_->set_world_x(world_x / world_num_inputs);
489  output_if_->set_world_y(world_y / world_num_inputs);
490  output_if_->set_world_z(world_z / world_num_inputs);
491  }
492  if (euler_num_inputs > 0) {
493  output_if_->set_roll(roll / euler_num_inputs);
494  output_if_->set_pitch(pitch / euler_num_inputs);
495  output_if_->set_yaw(yaw / euler_num_inputs);
496  }
497  if (worldvel_num_inputs > 0) {
498  output_if_->set_world_x_velocity(world_x_velocity / worldvel_num_inputs);
499  output_if_->set_world_y_velocity(world_y_velocity / worldvel_num_inputs);
500  output_if_->set_world_z_velocity(world_z_velocity / worldvel_num_inputs);
501  }
502 
503  if (extent_num_inputs > 0) {
504  output_if_->set_extent_x(extent_x / extent_num_inputs);
505  output_if_->set_extent_y(extent_y / extent_num_inputs);
506  output_if_->set_extent_z(extent_z / extent_num_inputs);
507  }
508  if (relcart_num_inputs > 0) {
509  output_if_->set_relative_x(relative_x / relcart_num_inputs);
510  output_if_->set_relative_y(relative_y / relcart_num_inputs);
511  output_if_->set_relative_z(relative_z / relcart_num_inputs);
512  output_if_->set_relative_x_velocity(relative_x_velocity / relcart_num_inputs);
513  output_if_->set_relative_y_velocity(relative_y_velocity / relcart_num_inputs);
514  output_if_->set_relative_z_velocity(relative_z_velocity / relcart_num_inputs);
515  }
516  if (relpolar_num_inputs > 0) {
517  output_if_->set_distance(distance / (float)relpolar_num_inputs);
518  output_if_->set_bearing(bearing / (float)relpolar_num_inputs);
519  output_if_->set_slope(slope / (float)relpolar_num_inputs);
520  }
521 
522  visible = have_world || have_relative;
523 
524  output_if_->set_flags(flags);
525  output_if_->set_valid(valid);
526  output_if_->set_visible(visible);
527  output_if_->set_visibility_history(visible ? vishistory_max : vishistory_min);
528 
529  output_if_->write();
530 }
531 
void set_world_y_velocity(const float new_world_y_velocity)
Set world_y_velocity value.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:211
void set_world_z_velocity(const float new_world_z_velocity)
Set world_z_velocity value.
~WorldModelObjPosMajorityFuser()
Destructor.
void set_world_x_velocity(const float new_world_x_velocity)
Set world_x_velocity value.
void set_relative_y_velocity(const float new_relative_y_velocity)
Set relative_y_velocity value.
void unlock() const
Unlock list.
Definition: lock_set.h:120
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:62
void set_valid(const bool new_valid)
Set valid value.
ObjectPositionInterface Fawkes BlackBoard Interface.
static const uint32_t FLAG_HAS_EULER_ANGLES
FLAG_HAS_EULER_ANGLES constant.
void set_world_z(const float new_world_z)
Set world_z value.
void set_extent_y(const float new_extent_y)
Set extent_y value.
void set_flags(const uint32_t new_flags)
Set flags value.
uint32_t flags() const
Get flags value.
void set_distance(const float new_distance)
Set distance value.
void set_roll(const float new_roll)
Set roll value.
void set_relative_y(const float new_relative_y)
Set relative_y value.
static const uint32_t FLAG_HAS_WORLD
FLAG_HAS_WORLD constant.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:495
void lock() const
Lock list.
Definition: lock_set.h:100
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:782
const char * id() const
Get identifier of interface.
Definition: interface.cpp:645
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
static const uint32_t FLAG_HAS_RELATIVE_POLAR
FLAG_HAS_RELATIVE_POLAR constant.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_relative_x_velocity(const float new_relative_x_velocity)
Set relative_x_velocity value.
bool is_visible() const
Get visible value.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
void set_relative_z_velocity(const float new_relative_z_velocity)
Set relative_z_velocity value.
static const uint32_t FLAG_HAS_EXTENT
FLAG_HAS_EXTENT constant.
void set_pitch(const float new_pitch)
Set pitch value.
void set_extent_z(const float new_extent_z)
Set extent_z value.
void set_extent_x(const float new_extent_x)
Set extent_x value.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:224
float distance() const
Get distance value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_visible(const bool new_visible)
Set visible value.
void set_bearing(const float new_bearing)
Set bearing value.
static const uint32_t FLAG_HAS_RELATIVE_CARTESIAN
FLAG_HAS_RELATIVE_CARTESIAN constant.
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
void set_slope(const float new_slope)
Set slope value.
void set_world_x(const float new_world_x)
Set world_x value.
void set_relative_x(const float new_relative_x)
Set relative_x value.
WorldModelObjPosMajorityFuser(fawkes::Logger *logger, fawkes::BlackBoard *blackboard, const std::string &own_id, const std::string &foreign_id_pattern, const std::string &output_id, float self_confidence_radius)
Constructor.
bool is_valid() const
Get valid value.
static const uint32_t FLAG_HAS_WORLD_VELOCITY
FLAG_HAS_WORLD_VELOCITY constant.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
std::set< OpiWrapper, std::less< OpiWrapper > >::iterator iterator
Iterator.
Definition: lock_set.h:49
virtual void fuse()
The single function that makes fusers work.
The BlackBoard abstract class.
Definition: blackboard.h:49
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*")=0
Open multiple interfaces for reading.
void set_yaw(const float new_yaw)
Set yaw value.
virtual void copy_values(const Interface *other)
Copy values from other interface.
void set_relative_z(const float new_relative_z)
Set relative_z value.
void set_world_y(const float new_world_y)
Set world_y value.
virtual void close(Interface *interface)=0
Close interface.
Interface for logging.
Definition: logger.h:34