Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
objpos_average.cpp
1 
2 /***************************************************************************
3  * objpos_average.cpp - Fawkes WorldModel Object Position Average Fuser
4  *
5  * Created: Tue Jan 13 13:51:45 2009
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
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 #include "objpos_average.h"
24 
25 #include <core/threading/mutex_locker.h>
26 #include <blackboard/blackboard.h>
27 #include <logging/logger.h>
28 #include <interfaces/ObjectPositionInterface.h>
29 
30 #include <cstring>
31 
32 using namespace fawkes;
33 
34 /** @class WorldModelObjPosAverageFuser "objpos_average.h"
35  * ObjectPositionModel average fuser.
36  * This fuser takes a number of ObjectPositionInterface instanced and fuses them
37  * into a single ObjectPositionInterface by averaging over the source interfaces.
38  * It registers as an observer and opens any newly created interface that matches
39  * the ID pattern.
40  * @author Tim Niemueller
41  */
42 
43 /** Constructor.
44  * @param blackboard BlackBoard
45  * @param from_id_pattern pattern for ID of the interfaces to copy from
46  * @param to_id ID of the interface to copy to
47  * @param logger logger
48  */
50  fawkes::BlackBoard *blackboard,
51  const char *from_id_pattern,
52  const char *to_id)
53 {
54  __logger = logger;
55  __blackboard = blackboard;
56  __to_id = to_id;
57 
58  __input_ifs.clear();
59  __output_if = NULL;
60  try {
61  __input_ifs = blackboard->open_multiple_for_reading<ObjectPositionInterface>(from_id_pattern);
62  __output_if = blackboard->open_for_writing<ObjectPositionInterface>(to_id);
63 
64  // If our output interface was already opened open_multiple might have opened
65  // it as well, check and close if that was the case
66  for (LockList<ObjectPositionInterface *>::iterator i = __input_ifs.begin(); i != __input_ifs.end(); ++i) {
67  if (__to_id == (*i)->id()) {
68  blackboard->close(*i);
69  __input_ifs.erase(i);
70  break;
71  }
72  }
73  } catch (Exception &e) {
74  for (LockList<ObjectPositionInterface *>::iterator i = __input_ifs.begin(); i != __input_ifs.end(); ++i) {
75  blackboard->close(*i);
76  }
77  __input_ifs.clear();
78  blackboard->close(__output_if);
79  throw;
80  }
81 
82  bbio_add_observed_create("ObjectPositionInterface", from_id_pattern);
83  blackboard->register_observer(this);
84 }
85 
86 
87 /** Destructor. */
89 {
90  __blackboard->unregister_observer(this);
91 
92  __input_ifs.lock();
93  for (__iii = __input_ifs.begin(); __iii != __input_ifs.end(); ++__iii) {
94  __blackboard->close(*__iii);
95  }
96  __input_ifs.clear();
97  __input_ifs.unlock();
98 
99  __blackboard->close(__output_if);
100 }
101 
102 
103 void
104 WorldModelObjPosAverageFuser::bb_interface_created(const char *type, const char *id) throw()
105 {
106  if (__to_id == id) return;
107 
108  ObjectPositionInterface *from_if = NULL;
109 
110  try {
111  from_if = __blackboard->open_for_reading<ObjectPositionInterface>(id);
112 
113  __input_ifs.push_back_locked(from_if);
114  } catch (Exception &e) {
115  if (from_if != NULL) {
116  __blackboard->close(from_if);
117  }
118  e.print_trace();
119  }
120 }
121 
122 
123 void
125 {
126  MutexLocker lock(__input_ifs.mutex());
127 
128  unsigned int flags = 0;
129  unsigned int base_flags = 0;
130  unsigned int world_num_inputs=0, extent_num_inputs=0, euler_num_inputs = 0,
131  worldvel_num_inputs = 0, relcart_num_inputs = 0, relpolar_num_inputs = 0;
132  float roll = 0, pitch = 0, yaw = 0, distance = 0, bearing = 0, slope = 0,
133  world_x = 0, world_y = 0, world_z = 0,
134  relative_x = 0, relative_y = 0, relative_z = 0,
135  extent_x = 0, extent_y = 0, extent_z = 0,
136  world_x_velocity = 0, world_y_velocity = 0, world_z_velocity = 0,
137  relative_x_velocity = 0, relative_y_velocity = 0, relative_z_velocity = 0;
138  bool valid = true, visible = true;
139  int vishistory_min = 0, vishistory_max = 0;
140  unsigned int object_type = 0;
141  bool object_type_warned = false;
142  bool flags_read = false;
143  bool have_world = false, have_relative = false;
144 
145  for (__iii = __input_ifs.begin(); __iii != __input_ifs.end(); ++__iii) {
146  ObjectPositionInterface *iface = *__iii;
147  if (iface->has_writer()) {
148  iface->read();
149  if (iface->is_valid()) {
150  if ( (object_type != 0) && (iface->object_type() != object_type) &&
151  ! object_type_warned) {
152  __logger->log_warn("WMObjPosAvgFus", "Object types of input interfaces "
153  "for %s disagree, %s has %u, expected was %u",
154  __to_id.c_str(), iface->uid(), iface->object_type(),
155  object_type);
156  object_type_warned = true;
157  } else {
158  object_type = iface->object_type();
159  }
160 
161  if (flags_read) {
162  unsigned int iflags = iface->flags()
166  if (iflags != base_flags) {
167  __logger->log_warn("WMObjPosAvgFus", "Interface flags for %s "
168  "disagree. Exected %x, got %x", base_flags, iflags);
169  }
170  } else {
171  base_flags = iface->flags()
175  flags_read = true;
176  }
177 
178  if (iface->is_visible()) {
180  have_world = true;
181 
183  world_x += iface->world_x();
184  world_y += iface->world_y();
185  world_z += iface->world_z();
186  world_num_inputs += 1;
187 
189  roll += iface->roll();
190  pitch += iface->pitch();
191  yaw += iface->yaw();
193  euler_num_inputs += 1;
194  }
195 
197  world_x_velocity += iface->world_x_velocity();
198  world_y_velocity += iface->world_y_velocity();
199  world_z_velocity += iface->world_z_velocity();
201  worldvel_num_inputs += 1;
202  }
203  }
204 
206  have_relative = true;
207 
209 
210  relative_x += iface->relative_x();
211  relative_y += iface->relative_y();
212  relative_z += iface->relative_z();
213  relative_x_velocity += iface->relative_x_velocity();
214  relative_y_velocity += iface->relative_y_velocity();
215  relative_z_velocity += iface->relative_z_velocity();
216  relcart_num_inputs += 1;
217  }
218 
220  have_relative = true;
221 
223 
224  distance += iface->distance();
225  bearing += iface->bearing();
226  slope += iface->slope();
227  relpolar_num_inputs += 1;
228  }
229 
231  extent_x += iface->extent_x();
232  extent_y += iface->extent_y();
233  extent_z += iface->extent_z();
235  extent_num_inputs += 1;
236  }
237 
238  if (iface->visibility_history() > vishistory_max) {
239  vishistory_max = iface->visibility_history();
240  }
241  } else {
242  if (iface->visibility_history() < vishistory_min) {
243  vishistory_min = iface->visibility_history();
244  }
245  }
246  }
247  }
248  }
249 
250  if ( world_num_inputs > 0 ) {
251  __output_if->set_world_x(world_x / (float)world_num_inputs);
252  __output_if->set_world_y(world_y / (float)world_num_inputs);
253  __output_if->set_world_z(world_z / (float)world_num_inputs);
254  }
255  if ( euler_num_inputs > 0 ) {
256  __output_if->set_roll(roll / (float)euler_num_inputs);
257  __output_if->set_pitch(pitch / (float)euler_num_inputs);
258  __output_if->set_yaw(yaw / (float)euler_num_inputs);
259  }
260  if ( worldvel_num_inputs > 0) {
261  __output_if->set_world_x_velocity(world_x_velocity / (float)worldvel_num_inputs);
262  __output_if->set_world_y_velocity(world_y_velocity / (float)worldvel_num_inputs);
263  __output_if->set_world_z_velocity(world_z_velocity / (float)worldvel_num_inputs);
264  }
265 
266  if ( extent_num_inputs > 0 ) {
267  __output_if->set_extent_x(extent_x / (float)extent_num_inputs);
268  __output_if->set_extent_y(extent_y / (float)extent_num_inputs);
269  __output_if->set_extent_z(extent_z / (float)extent_num_inputs);
270  }
271  if ( relcart_num_inputs > 0) {
272  __output_if->set_relative_x(relative_x / (float)relcart_num_inputs);
273  __output_if->set_relative_y(relative_y / (float)relcart_num_inputs);
274  __output_if->set_relative_z(relative_z / (float)relcart_num_inputs);
275  __output_if->set_relative_x_velocity(relative_x_velocity / (float)relcart_num_inputs);
276  __output_if->set_relative_y_velocity(relative_y_velocity / (float)relcart_num_inputs);
277  __output_if->set_relative_z_velocity(relative_z_velocity / (float)relcart_num_inputs);
278  }
279  if ( relpolar_num_inputs > 0) {
280  __output_if->set_distance(distance / (float)relpolar_num_inputs);
281  __output_if->set_bearing(bearing / (float)relpolar_num_inputs);
282  __output_if->set_slope(slope / (float)relpolar_num_inputs);
283  }
284 
285  visible = have_world || have_relative;
286 
287  __output_if->set_flags(flags);
288  __output_if->set_valid(valid);
289  __output_if->set_visible(visible);
290  __output_if->set_visibility_history(visible ? vishistory_max : vishistory_min);
291 
292  __output_if->write();
293 }