Fawkes API Fawkes Development Version

objpos_majority.cpp

00001 
00002 /***************************************************************************
00003  *  objpos_majority.cpp - Fawkes WorldModel Object Position Majority Fuser
00004  *
00005  *  Created: Thu 01 Apr 2010 05:06:36 PM CEST
00006  *  Copyright  2010  Christoph Schwering
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.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "objpos_majority.h"
00024 
00025 #include <iostream>
00026 #include <cmath>
00027 #include <cstring>
00028 #include <list>
00029 
00030 #include <core/threading/mutex_locker.h>
00031 #include <blackboard/blackboard.h>
00032 #include <utils/logging/logger.h>
00033 
00034 /** @class WorldModelObjPosMajorityFuser "objpos_majority.h"
00035  * ObjectPositionInterface majority fuser.
00036  * The parameters are (1) the ID of the own ObjectPositionInterface, (2) the
00037  * pattern ID of the other robots' ObjectPositionInterfaces and (3) the
00038  * maximum-self-confidence-distance.
00039  *
00040  * (1) If the own ObjectPositionInterface thinks the object is not further away
00041  * than self_confidence_radius, then the own interface's data is copied to the
00042  * output interface.
00043  * (2) If there is an unambiguous majority of interfaces that say the object is
00044  * somewhere else and this majority is averaged and the average values are
00045  * copied to the output interface.
00046  * Since the other interfaces probably won't agree on one exact position, they
00047  * are grouped: for each interface A its group is the set of interfaces that
00048  * claim the object is not further away from the position claimed by A than
00049  * GROUP_RADIUS. GROUP_RADIUS is currently hard-coded to 1.0 meters.
00050  * (3) If the other interfaces "cannot settle" on some position of the object,
00051  * the own interface's data is considered as at least as reliable as theirs and
00052  * therefore the own interface's data is copied to the output interface.
00053  *
00054  * Like the WorldModelObjPosMajorityFuser, it registers as an observer and opens
00055  * any newly created interface that matches the ID of the own
00056  * ObjectPositionInterface or the pattern of the foreign
00057  * ObjectPositionInterfaces.
00058  * @author Christoph Schwering
00059  */
00060 
00061 /** Constructor.
00062  * @param blackboard BlackBoard.
00063  * @param logger Logger.
00064  * @param own_id The ID of the (single) own interface.
00065  * @param foreign_id_pattern The pattern of the (multiple) other interfaces.
00066  * @param output_id The ID of the destination interface.
00067  * @param self_confidence_radius radius in which to consider our perception
00068  * the best
00069  */
00070 WorldModelObjPosMajorityFuser::WorldModelObjPosMajorityFuser(
00071     fawkes::Logger* logger,
00072     fawkes::BlackBoard* blackboard,
00073     const std::string& own_id,
00074     const std::string& foreign_id_pattern,
00075     const std::string& output_id,
00076     float self_confidence_radius)
00077     : logger_(logger),
00078       blackboard_(blackboard),
00079       own_id_(own_id),
00080       output_id_(output_id),
00081       self_confidence_radius_(self_confidence_radius)
00082 {
00083   input_ifs_.clear();
00084   output_if_ = NULL;
00085   try {
00086     own_if_ = blackboard_->open_for_reading<Opi>(own_id.c_str());
00087     std::list<Opi*> input_ifs = blackboard_->open_multiple_for_reading<Opi>(
00088         foreign_id_pattern.c_str());
00089     for (std::list<Opi*>::const_iterator it = input_ifs.begin();
00090          it != input_ifs.end(); ++it) {
00091       Opi* opi = *it;
00092       std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert(opi);
00093       if (!ret.second) {
00094         blackboard->close(opi);
00095       }
00096     }
00097     if (own_if_ != NULL) {
00098       std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert(own_if_);
00099       if (!ret.second) {
00100         blackboard->close(own_if_);
00101         own_if_ = *ret.first;
00102       }
00103     }
00104 
00105     output_if_ = blackboard_->open_for_writing<Opi>(output_id.c_str());
00106     OpiSet::iterator iter = input_ifs_.find(output_if_);
00107     if (iter != input_ifs_.end()) {
00108       Opi* opi = *iter;
00109       blackboard->close(opi);
00110       input_ifs_.erase(iter);
00111     }
00112   } catch (fawkes::Exception& e) {
00113     for (OpiSet::const_iterator it = input_ifs_.begin();
00114          it != input_ifs_.end(); ++it) {
00115       blackboard->close(*it);
00116     }
00117     input_ifs_.clear();
00118     if (output_if_ != NULL) {
00119       blackboard->close(output_if_);
00120       output_if_ = NULL;
00121     }
00122     throw;
00123   }
00124 
00125   bbio_add_observed_create("ObjectPositionInterface",
00126                            own_id.c_str());
00127   bbio_add_observed_create("ObjectPositionInterface",
00128                            foreign_id_pattern.c_str());
00129   blackboard_->register_observer(this, fawkes::BlackBoard::BBIO_FLAG_CREATED);
00130 }
00131 
00132 
00133 /** Destructor. */
00134 WorldModelObjPosMajorityFuser::~WorldModelObjPosMajorityFuser()
00135 {
00136   blackboard_->unregister_observer(this);
00137 
00138   input_ifs_.lock();
00139   for (OpiSet::const_iterator it = input_ifs_.begin();
00140        it != input_ifs_.end(); ++it) {
00141     blackboard_->close(*it);
00142   }
00143   input_ifs_.clear();
00144   input_ifs_.unlock();
00145 
00146   if (output_if_ != NULL) {
00147     blackboard_->close(output_if_);
00148   }
00149 }
00150 
00151 
00152 void
00153 WorldModelObjPosMajorityFuser::bb_interface_created(const char *type,
00154                                                     const char *id) throw()
00155 {
00156   if (output_id_ == id) {
00157     return;
00158   }
00159   Opi* from_if = NULL;
00160   try {
00161     from_if = blackboard_->open_for_reading<Opi>(id);
00162     std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert_locked(from_if);
00163     if (!ret.second) {
00164       blackboard_->close(from_if);
00165     }
00166     Opi* inserted_if = *ret.first;
00167     if (own_if_ == NULL && own_id_ == std::string(inserted_if->id())) {
00168       own_if_ = inserted_if;
00169     }
00170   } catch (fawkes::Exception& e) {
00171     if (from_if != NULL) {
00172       blackboard_->close(from_if);
00173     }
00174     e.print_trace();
00175   }
00176 }
00177 
00178 float
00179 WorldModelObjPosMajorityFuser::length(float x, float y, float z)
00180 {
00181   return sqrt(x*x + y*y + z*z);
00182 }
00183 
00184 float
00185 WorldModelObjPosMajorityFuser::rel_length(const Opi* opi)
00186 {
00187   return length(opi->relative_x(), opi->relative_y(), opi->relative_z());
00188 }
00189 
00190 float
00191 WorldModelObjPosMajorityFuser::world_object_dist(const Opi* from, const Opi* to)
00192 {
00193   return length(to->world_x() - from->world_x(),
00194                 to->world_y() - from->world_y(),
00195                 to->world_z() - from->world_z());
00196 }
00197 
00198 bool
00199 WorldModelObjPosMajorityFuser::same_contents(const OpiBucket& left,
00200                                              const OpiBucket& right)
00201 {
00202   if (left.size() != right.size()) {
00203     return false;
00204   }
00205 
00206   std::set<OpiWrapper> rightSet(right.begin(), right.end());
00207   for (OpiBucket::const_iterator it = left.begin();
00208        it != left.end(); ++it) {
00209     Opi* opi = *it;
00210     if (rightSet.find(opi) == rightSet.end()) {
00211       return false;
00212     }
00213   }
00214   return true;
00215 }
00216 
00217 void
00218 WorldModelObjPosMajorityFuser::fuse()
00219 {
00220   if (own_if_ != NULL) {
00221     own_if_->read();
00222   }
00223 
00224   if (own_if_ != NULL &&
00225       own_if_->has_writer() &&
00226       own_if_->is_valid() &&
00227       own_if_->is_visible() &&
00228       (((own_if_->flags() & Opi::FLAG_HAS_RELATIVE_CARTESIAN) &&
00229         rel_length(own_if_) <= self_confidence_radius_) ||
00230        ((own_if_->flags() & Opi::FLAG_HAS_RELATIVE_POLAR) &&
00231          own_if_->distance() <= self_confidence_radius_))) {
00232     // Case 1: just copy own data if the own data claims the ball is very near.
00233     copy_own_if();
00234 
00235   } else {
00236     // Case 2: group interfaces, look for a majority and average it, if there is
00237     // none, copy the own interface.
00238 
00239     for (OpiSet::const_iterator it = input_ifs_.begin();
00240          it != input_ifs_.end(); ++it) {
00241       Opi* opi = *it;
00242       if (opi != own_if_) { // we've read own_if_ already
00243         opi->read();
00244       }
00245     }
00246     check();
00247 
00248     // Group interfaces in buckets.
00249     input_ifs_.lock();
00250     OpiBuckets buckets;
00251     for (OpiSet::const_iterator it = input_ifs_.begin();
00252          it != input_ifs_.end(); ++it) {
00253       Opi* opi = *it;
00254       if (!(opi->flags() & Opi::FLAG_HAS_WORLD) ||
00255           !opi->has_writer() ||
00256           !opi->is_valid() ||
00257           !opi->is_visible()) {
00258         continue;
00259       }
00260       OpiBucket bucket;
00261       bucket.push_back(opi);
00262       for (OpiSet::const_iterator jt = input_ifs_.begin();
00263            jt != input_ifs_.end(); ++jt) {
00264         Opi* candidate = *jt;
00265         if (candidate != opi &&
00266             (candidate->flags() & Opi::FLAG_HAS_WORLD) &&
00267             world_object_dist(opi, candidate) <= GROUP_RADIUS) {
00268           bucket.push_back(candidate);
00269         }
00270       }
00271       buckets.push_back(bucket);
00272     }
00273     input_ifs_.unlock();
00274 
00275     // Search for majority.
00276     OpiBucket majority;
00277     bool unambiguous = false;
00278     for (OpiBuckets::const_iterator it = buckets.begin();
00279          it != buckets.end(); ++it) {
00280       const OpiBucket& bucket = *it;
00281       if (majority.size() <= bucket.size()) {
00282         if (majority.size() < bucket.size()) {
00283           majority = bucket;
00284           unambiguous = true;
00285         } else {
00286           unambiguous = unambiguous && same_contents(majority, bucket);
00287         }
00288       }
00289     }
00290     if (majority.size() > 0 && unambiguous) {
00291       // Case 2a: calculate average of majority.
00292       average(majority);
00293     } else {
00294       // Case 2b: no majority found, copy own data.
00295       copy_own_if();
00296     }
00297   }
00298 }
00299 
00300 void
00301 WorldModelObjPosMajorityFuser::check()
00302 {
00303   unsigned int base_flags = 0;
00304 
00305   unsigned int object_type = 0;
00306   bool object_type_warned = false;
00307   bool flags_read = false;
00308 
00309   for (OpiSet::const_iterator it = input_ifs_.begin();
00310        it != input_ifs_.end(); ++it) {
00311     Opi* opi = *it;
00312     if (!opi->has_writer()) {
00313       continue;
00314     }
00315     if (!opi->is_valid()) {
00316       continue;
00317     }
00318     if (object_type != 0 && opi->object_type() != object_type &&
00319         !object_type_warned) {
00320       logger_->log_warn("WMObjPosAvgFus", "Object types of input interfaces "
00321                         "for %s disagree, %s has %u, expected was %u",
00322                         output_id_.c_str(), opi->uid(), opi->object_type(),
00323                         object_type);
00324       object_type_warned = true;
00325     } else {
00326       object_type = opi->object_type();
00327     }
00328 
00329     if (flags_read) {
00330       unsigned int iflags = opi->flags()
00331         & (0xFFFFFFFF ^ Opi::FLAG_HAS_WORLD)
00332         & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_CARTESIAN)
00333         & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_POLAR);
00334       if (iflags != base_flags) {
00335         logger_->log_warn("WMObjPosAvgFus", "Interface flags for %s "
00336                           "disagree. Exected %x, got %x", base_flags,
00337                           iflags);
00338       }
00339     } else {
00340       base_flags = opi->flags()
00341         & (0xFFFFFFFF ^ Opi::FLAG_HAS_WORLD)
00342         & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_CARTESIAN)
00343         & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_POLAR);
00344       flags_read = true;
00345     }
00346   }
00347 }
00348 
00349 void
00350 WorldModelObjPosMajorityFuser::copy_own_if()
00351 {
00352   output_if_->copy_values(own_if_);
00353   output_if_->write();
00354 }
00355 
00356 /** Averages over the given input interfaces.
00357  * The same like WorldModelObjPosAverageFuser::fuse() except that this method
00358  * works on a parameter.
00359  * (Making this fuser a subclass of the average fuser would make sense.)
00360  * @param input_ifs The interface that are averaged.
00361  */
00362 void
00363 WorldModelObjPosMajorityFuser::average(const OpiBucket& input_ifs)
00364 {
00365   unsigned int flags = 0;
00366   unsigned int world_num_inputs = 0;
00367   unsigned int extent_num_inputs = 0;
00368   unsigned int euler_num_inputs = 0;
00369   unsigned int worldvel_num_inputs = 0;
00370   unsigned int relcart_num_inputs = 0;
00371   unsigned int relpolar_num_inputs = 0;
00372 
00373   float roll = 0.0f;
00374   float pitch = 0.0f;
00375   float yaw = 0.0f;
00376   float distance = 0.0f;
00377   float bearing = 0.0f;
00378   float slope = 0.0f;
00379 
00380   float world_x = 0.0f;
00381   float world_y = 0.0f;
00382   float world_z = 0.0f;
00383 
00384   float relative_x = 0.0f;
00385   float relative_y = 0.0f;
00386   float relative_z = 0.0f;
00387 
00388   float extent_x = 0.0f;
00389   float extent_y = 0.0f;
00390   float extent_z = 0.0f;
00391 
00392   float world_x_velocity = 0.0f;
00393   float world_y_velocity = 0.0f;
00394   float world_z_velocity = 0.0f;
00395 
00396   float relative_x_velocity = 0.0f;
00397   float relative_y_velocity = 0.0f;
00398   float relative_z_velocity = 0.0f;
00399 
00400   bool valid = true;
00401   bool visible = true;
00402   int vishistory_min = 0;
00403   int vishistory_max = 0;
00404   bool have_world = false, have_relative = false;
00405 
00406   for (OpiBucket::const_iterator it = input_ifs.begin();
00407        it != input_ifs.end(); ++it) {
00408     Opi* opi = *it;
00409     if (!opi->has_writer()) {
00410       continue;
00411     }
00412     opi->read();
00413     if (!opi->is_valid()) {
00414       continue;
00415     }
00416 
00417     if (opi->is_visible()) {
00418       if (opi->flags() & Opi::FLAG_HAS_WORLD) {
00419         have_world = true;
00420 
00421         flags |= Opi::FLAG_HAS_WORLD;
00422         world_x             += opi->world_x();
00423         world_y             += opi->world_y();
00424         world_z             += opi->world_z();
00425         world_num_inputs    += 1;
00426 
00427         if (opi->flags() & Opi::FLAG_HAS_EULER_ANGLES) {
00428           roll              += opi->roll();
00429           pitch             += opi->pitch();
00430           yaw               += opi->yaw();
00431           flags             |= Opi::FLAG_HAS_EULER_ANGLES;
00432           euler_num_inputs  += 1;
00433         }
00434 
00435         if (opi->flags() & Opi::FLAG_HAS_WORLD_VELOCITY) {
00436           world_x_velocity    += opi->world_x_velocity();
00437           world_y_velocity    += opi->world_y_velocity();
00438           world_z_velocity    += opi->world_z_velocity();
00439           flags               |= Opi::FLAG_HAS_WORLD_VELOCITY;
00440           worldvel_num_inputs += 1;
00441         }
00442       }
00443 
00444       if (opi->flags() & Opi::FLAG_HAS_RELATIVE_CARTESIAN) {
00445         have_relative = true;
00446 
00447         flags |= Opi::FLAG_HAS_RELATIVE_CARTESIAN;
00448         
00449         relative_x          += opi->relative_x();
00450         relative_y          += opi->relative_y();
00451         relative_z          += opi->relative_z();
00452         relative_x_velocity += opi->relative_x_velocity();
00453         relative_y_velocity += opi->relative_y_velocity();
00454         relative_z_velocity += opi->relative_z_velocity();
00455         relcart_num_inputs  += 1;
00456       }
00457 
00458       if (opi->flags() & Opi::FLAG_HAS_RELATIVE_POLAR) {
00459         have_relative = true;
00460         
00461         flags |= Opi::FLAG_HAS_RELATIVE_POLAR;
00462         
00463         distance            += opi->distance();
00464         bearing             += opi->bearing();
00465         slope               += opi->slope();
00466         relpolar_num_inputs += 1;
00467       }
00468 
00469       if (opi->flags() & Opi::FLAG_HAS_EXTENT) {
00470         extent_x          += opi->extent_x();
00471         extent_y          += opi->extent_y();
00472         extent_z          += opi->extent_z();
00473         flags             |= Opi::FLAG_HAS_EXTENT;
00474         extent_num_inputs += 1;
00475       }
00476 
00477       if (opi->visibility_history() > vishistory_max) {
00478         vishistory_max = opi->visibility_history();
00479       }
00480     } else {
00481       if (opi->visibility_history() < vishistory_min) {
00482         vishistory_min = opi->visibility_history();
00483       }
00484     }
00485   }
00486 
00487   if (world_num_inputs > 0) {
00488     output_if_->set_world_x(world_x / world_num_inputs);
00489     output_if_->set_world_y(world_y / world_num_inputs);
00490     output_if_->set_world_z(world_z / world_num_inputs);
00491   }
00492   if (euler_num_inputs > 0) {
00493     output_if_->set_roll(roll / euler_num_inputs);
00494     output_if_->set_pitch(pitch  / euler_num_inputs);
00495     output_if_->set_yaw(yaw / euler_num_inputs);
00496   }
00497   if (worldvel_num_inputs > 0) {
00498     output_if_->set_world_x_velocity(world_x_velocity / worldvel_num_inputs);
00499     output_if_->set_world_y_velocity(world_y_velocity / worldvel_num_inputs);
00500     output_if_->set_world_z_velocity(world_z_velocity / worldvel_num_inputs);
00501   }
00502 
00503   if (extent_num_inputs > 0) {
00504     output_if_->set_extent_x(extent_x / extent_num_inputs);
00505     output_if_->set_extent_y(extent_y / extent_num_inputs);
00506     output_if_->set_extent_z(extent_z / extent_num_inputs);
00507   }
00508   if (relcart_num_inputs > 0) {
00509     output_if_->set_relative_x(relative_x / relcart_num_inputs);
00510     output_if_->set_relative_y(relative_y / relcart_num_inputs);
00511     output_if_->set_relative_z(relative_z / relcart_num_inputs);
00512     output_if_->set_relative_x_velocity(relative_x_velocity / relcart_num_inputs);
00513     output_if_->set_relative_y_velocity(relative_y_velocity / relcart_num_inputs);
00514     output_if_->set_relative_z_velocity(relative_z_velocity / relcart_num_inputs);
00515   }
00516   if (relpolar_num_inputs > 0) {
00517     output_if_->set_distance(distance / (float)relpolar_num_inputs);
00518     output_if_->set_bearing(bearing / (float)relpolar_num_inputs);
00519     output_if_->set_slope(slope / (float)relpolar_num_inputs);
00520   }
00521 
00522   visible = have_world || have_relative;
00523 
00524   output_if_->set_flags(flags);
00525   output_if_->set_valid(valid);
00526   output_if_->set_visible(visible);
00527   output_if_->set_visibility_history(visible ? vishistory_max : vishistory_min);
00528 
00529   output_if_->write();
00530 }
00531 
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends