Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * objpos_majority.h - 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 #ifndef __PLUGINS_WORLDMODEL_FUSER_OBJPOS_MAJORITY_H_ 00024 #define __PLUGINS_WORLDMODEL_FUSER_OBJPOS_MAJORITY_H_ 00025 00026 #include <cassert> 00027 #include <cstring> 00028 #include <string> 00029 #include <vector> 00030 00031 #include <blackboard/interface_observer.h> 00032 #include <core/utils/lock_set.h> 00033 #include <interfaces/ObjectPositionInterface.h> 00034 00035 #include "fuser.h" 00036 00037 namespace fawkes 00038 { 00039 class BlackBoard; 00040 class Logger; 00041 class ObjectPositionInterface; 00042 } 00043 00044 class WorldModelObjPosMajorityFuser 00045 : public WorldModelFuser, 00046 public fawkes::BlackBoardInterfaceObserver 00047 { 00048 public: 00049 WorldModelObjPosMajorityFuser(fawkes::Logger* logger, 00050 fawkes::BlackBoard* blackboard, 00051 const std::string& own_id, 00052 const std::string& foreign_id_pattern, 00053 const std::string& output_id, 00054 float self_confidence_radius); 00055 ~WorldModelObjPosMajorityFuser(); 00056 00057 virtual void bb_interface_created(const char *type, const char *id) throw(); 00058 virtual void fuse(); 00059 00060 private: 00061 typedef fawkes::ObjectPositionInterface Opi; 00062 00063 /** Wrapper that compares by the Opi's id(). */ 00064 class OpiWrapper { 00065 public: 00066 OpiWrapper(Opi* opi) : opi_(opi) { assert(opi != NULL); } 00067 operator Opi*() const { return opi_; } 00068 00069 bool operator == (const OpiWrapper& o) const { return cmp(o) == 0; } 00070 bool operator < (const OpiWrapper& o) const { return cmp(o) < 0; } 00071 00072 Opi* opi() { return opi_; } 00073 const Opi* opi() const { return opi_; } 00074 00075 private: 00076 int cmp(const OpiWrapper& o) const { return strcmp(opi_->id(), 00077 o.opi_->id()); } 00078 Opi* opi_; 00079 }; 00080 00081 typedef fawkes::LockSet<OpiWrapper> OpiSet; 00082 typedef std::vector<Opi*> OpiBucket; 00083 typedef std::vector<OpiBucket> OpiBuckets; 00084 00085 const static float GROUP_RADIUS = 1.0f; 00086 00087 void check(); 00088 void copy_own_if(); 00089 void average(const OpiBucket& input_ifs); 00090 00091 static float length(float x, float y, float z); 00092 static float rel_length(const Opi* iface); 00093 static float world_object_dist(const Opi* from, const Opi* to); 00094 static bool same_contents(const OpiBucket& left, const OpiBucket& right); 00095 00096 fawkes::Logger *logger_; 00097 fawkes::BlackBoard *blackboard_; 00098 00099 std::string own_id_; 00100 std::string output_id_; 00101 00102 float self_confidence_radius_; 00103 00104 Opi* own_if_; 00105 OpiSet input_ifs_; 00106 Opi* output_if_; 00107 }; 00108 00109 #endif 00110