objpos_majority.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
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