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 /** Wrapper creator. 00067 * @param opi Object position interface to compare to */ 00068 OpiWrapper(Opi* opi) : opi_(opi) { assert(opi != NULL); } 00069 /** Dereferencing operator. 00070 * @return pointer to object position interface. */ 00071 operator Opi*() const { return opi_; } 00072 00073 /** Equality operator. 00074 * @param o other object position wrapper to compare to 00075 * @return true of the interfaces are the same, false otherwise 00076 */ 00077 bool operator == (const OpiWrapper& o) const { return cmp(o) == 0; } 00078 00079 /** Less than operator. 00080 * @param o other object position wrapper to compare to 00081 * @return true of the the given interface is small than this one 00082 */ 00083 bool operator < (const OpiWrapper& o) const { return cmp(o) < 0; } 00084 00085 /** Call operator. 00086 * @return wrapped interface */ 00087 Opi* opi() { return opi_; } 00088 00089 /** Const call operator. 00090 * @return const wrapped interface */ 00091 const Opi* opi() const { return opi_; } 00092 00093 private: 00094 int cmp(const OpiWrapper& o) const { return strcmp(opi_->id(), 00095 o.opi_->id()); } 00096 Opi* opi_; 00097 }; 00098 00099 typedef fawkes::LockSet<OpiWrapper> OpiSet; 00100 typedef std::vector<Opi*> OpiBucket; 00101 typedef std::vector<OpiBucket> OpiBuckets; 00102 00103 const static float GROUP_RADIUS = 1.0f; 00104 00105 void check(); 00106 void copy_own_if(); 00107 void average(const OpiBucket& input_ifs); 00108 00109 static float length(float x, float y, float z); 00110 static float rel_length(const Opi* iface); 00111 static float world_object_dist(const Opi* from, const Opi* to); 00112 static bool same_contents(const OpiBucket& left, const OpiBucket& right); 00113 00114 fawkes::Logger *logger_; 00115 fawkes::BlackBoard *blackboard_; 00116 00117 std::string own_id_; 00118 std::string output_id_; 00119 00120 float self_confidence_radius_; 00121 00122 Opi* own_if_; 00123 OpiSet input_ifs_; 00124 Opi* output_if_; 00125 }; 00126 00127 #endif 00128