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