Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
objpos_majority.h
1 
2 /***************************************************************************
3  * objpos_majority.h - Fawkes WorldModel Object Position Majority Fuser
4  *
5  * Created: Thu 01 Apr 2010 05:06:36 PM CEST
6  * Copyright 2010 Christoph Schwering
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef __PLUGINS_WORLDMODEL_FUSER_OBJPOS_MAJORITY_H_
24 #define __PLUGINS_WORLDMODEL_FUSER_OBJPOS_MAJORITY_H_
25 
26 #include <cassert>
27 #include <cstring>
28 #include <string>
29 #include <vector>
30 
31 #include <blackboard/interface_observer.h>
32 #include <core/utils/lock_set.h>
33 #include <interfaces/ObjectPositionInterface.h>
34 
35 #include "fuser.h"
36 
37 namespace fawkes
38 {
39  class BlackBoard;
40  class Logger;
41  class ObjectPositionInterface;
42 }
43 
45 : public WorldModelFuser,
47 {
48  public:
50  fawkes::BlackBoard* blackboard,
51  const std::string& own_id,
52  const std::string& foreign_id_pattern,
53  const std::string& output_id,
54  float self_confidence_radius);
56 
57  virtual void bb_interface_created(const char *type, const char *id) throw();
58  virtual void fuse();
59 
60  private:
62 
63  /** Wrapper that compares by the Opi's id(). */
64  class OpiWrapper {
65  public:
66  /** Wrapper creator.
67  * @param opi Object position interface to compare to */
68  OpiWrapper(Opi* opi) : opi_(opi) { assert(opi != NULL); }
69  /** Dereferencing operator.
70  * @return pointer to object position interface. */
71  operator Opi*() const { return opi_; }
72 
73  /** Equality operator.
74  * @param o other object position wrapper to compare to
75  * @return true of the interfaces are the same, false otherwise
76  */
77  bool operator == (const OpiWrapper& o) const { return cmp(o) == 0; }
78 
79  /** Less than operator.
80  * @param o other object position wrapper to compare to
81  * @return true of the the given interface is small than this one
82  */
83  bool operator < (const OpiWrapper& o) const { return cmp(o) < 0; }
84 
85  /** Call operator.
86  * @return wrapped interface */
87  Opi* opi() { return opi_; }
88 
89  /** Const call operator.
90  * @return const wrapped interface */
91  const Opi* opi() const { return opi_; }
92 
93  private:
94  int cmp(const OpiWrapper& o) const { return strcmp(opi_->id(),
95  o.opi_->id()); }
96  Opi* opi_;
97  };
98 
100  typedef std::vector<Opi*> OpiBucket;
101  typedef std::vector<OpiBucket> OpiBuckets;
102 
103  const static float GROUP_RADIUS = 1.0f;
104 
105  void check();
106  void copy_own_if();
107  void average(const OpiBucket& input_ifs);
108 
109  static float length(float x, float y, float z);
110  static float rel_length(const Opi* iface);
111  static float world_object_dist(const Opi* from, const Opi* to);
112  static bool same_contents(const OpiBucket& left, const OpiBucket& right);
113 
114  fawkes::Logger *logger_;
115  fawkes::BlackBoard *blackboard_;
116 
117  std::string own_id_;
118  std::string output_id_;
119 
120  float self_confidence_radius_;
121 
122  Opi* own_if_;
123  OpiSet input_ifs_;
124  Opi* output_if_;
125 };
126 
127 #endif
128 
~WorldModelObjPosMajorityFuser()
Destructor.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
ObjectPositionInterface Fawkes BlackBoard Interface.
ObjectPositionInterface majority fuser.
BlackBoard interface observer.
WorldModelObjPosMajorityFuser(fawkes::Logger *logger, fawkes::BlackBoard *blackboard, const std::string &own_id, const std::string &foreign_id_pattern, const std::string &output_id, float self_confidence_radius)
Constructor.
virtual void fuse()
The single function that makes fusers work.
The BlackBoard abstract class.
Definition: blackboard.h:49
Interface for data fusers for the world model.
Definition: fuser.h:26
Interface for logging.
Definition: logger.h:34