Fawkes API
Fawkes Development Version
|
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