objpos_majority.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
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
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
00233 copy_own_if();
00234
00235 } else {
00236
00237
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_) {
00243 opi->read();
00244 }
00245 }
00246 check();
00247
00248
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
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
00292 average(majority);
00293 } else {
00294
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
00357
00358
00359
00360
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