objpos_average.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "objpos_average.h"
00024
00025 #include <core/threading/mutex_locker.h>
00026 #include <blackboard/blackboard.h>
00027 #include <utils/logging/logger.h>
00028 #include <interfaces/ObjectPositionInterface.h>
00029
00030 #include <cstring>
00031
00032 using namespace fawkes;
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 WorldModelObjPosAverageFuser::WorldModelObjPosAverageFuser(fawkes::Logger *logger,
00050 fawkes::BlackBoard *blackboard,
00051 const char *from_id_pattern,
00052 const char *to_id)
00053 {
00054 __logger = logger;
00055 __blackboard = blackboard;
00056 __to_id = to_id;
00057
00058 __input_ifs.clear();
00059 __output_if = NULL;
00060 try {
00061 __input_ifs = blackboard->open_multiple_for_reading<ObjectPositionInterface>(from_id_pattern);
00062 __output_if = blackboard->open_for_writing<ObjectPositionInterface>(to_id);
00063
00064
00065
00066 for (LockList<ObjectPositionInterface *>::iterator i = __input_ifs.begin(); i != __input_ifs.end(); ++i) {
00067 if (__to_id == (*i)->id()) {
00068 blackboard->close(*i);
00069 __input_ifs.erase(i);
00070 break;
00071 }
00072 }
00073 } catch (Exception &e) {
00074 for (LockList<ObjectPositionInterface *>::iterator i = __input_ifs.begin(); i != __input_ifs.end(); ++i) {
00075 blackboard->close(*i);
00076 }
00077 __input_ifs.clear();
00078 blackboard->close(__output_if);
00079 throw;
00080 }
00081
00082 bbio_add_observed_create("ObjectPositionInterface", from_id_pattern);
00083 blackboard->register_observer(this, BlackBoard::BBIO_FLAG_CREATED);
00084 }
00085
00086
00087
00088 WorldModelObjPosAverageFuser::~WorldModelObjPosAverageFuser()
00089 {
00090 __blackboard->unregister_observer(this);
00091
00092 __input_ifs.lock();
00093 for (__iii = __input_ifs.begin(); __iii != __input_ifs.end(); ++__iii) {
00094 __blackboard->close(*__iii);
00095 }
00096 __input_ifs.clear();
00097 __input_ifs.unlock();
00098
00099 __blackboard->close(__output_if);
00100 }
00101
00102
00103 void
00104 WorldModelObjPosAverageFuser::bb_interface_created(const char *type, const char *id) throw()
00105 {
00106 if (__to_id == id) return;
00107
00108 ObjectPositionInterface *from_if = NULL;
00109
00110 try {
00111 from_if = __blackboard->open_for_reading<ObjectPositionInterface>(id);
00112
00113 __input_ifs.push_back_locked(from_if);
00114 } catch (Exception &e) {
00115 if (from_if != NULL) {
00116 __blackboard->close(from_if);
00117 }
00118 e.print_trace();
00119 }
00120 }
00121
00122
00123 void
00124 WorldModelObjPosAverageFuser::fuse()
00125 {
00126 MutexLocker lock(__input_ifs.mutex());
00127
00128 unsigned int flags = 0;
00129 unsigned int base_flags = 0;
00130 unsigned int world_num_inputs=0, extent_num_inputs=0, euler_num_inputs = 0,
00131 worldvel_num_inputs = 0, relcart_num_inputs = 0, relpolar_num_inputs = 0;
00132 float roll = 0, pitch = 0, yaw = 0, distance = 0, bearing = 0, slope = 0,
00133 world_x = 0, world_y = 0, world_z = 0,
00134 relative_x = 0, relative_y = 0, relative_z = 0,
00135 extent_x = 0, extent_y = 0, extent_z = 0,
00136 world_x_velocity = 0, world_y_velocity = 0, world_z_velocity = 0,
00137 relative_x_velocity = 0, relative_y_velocity = 0, relative_z_velocity = 0;
00138 bool valid = true, visible = true;
00139 int vishistory_min = 0, vishistory_max = 0;
00140 unsigned int object_type = 0;
00141 bool object_type_warned = false;
00142 bool flags_read = false;
00143 bool have_world = false, have_relative = false;
00144
00145 for (__iii = __input_ifs.begin(); __iii != __input_ifs.end(); ++__iii) {
00146 ObjectPositionInterface *iface = *__iii;
00147 if (iface->has_writer()) {
00148 iface->read();
00149 if (iface->is_valid()) {
00150 if ( (object_type != 0) && (iface->object_type() != object_type) &&
00151 ! object_type_warned) {
00152 __logger->log_warn("WMObjPosAvgFus", "Object types of input interfaces "
00153 "for %s disagree, %s has %u, expected was %u",
00154 __to_id.c_str(), iface->uid(), iface->object_type(),
00155 object_type);
00156 object_type_warned = true;
00157 } else {
00158 object_type = iface->object_type();
00159 }
00160
00161 if (flags_read) {
00162 unsigned int iflags = iface->flags()
00163 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_WORLD)
00164 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN)
00165 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR);
00166 if (iflags != base_flags) {
00167 __logger->log_warn("WMObjPosAvgFus", "Interface flags for %s "
00168 "disagree. Exected %x, got %x", base_flags, iflags);
00169 }
00170 } else {
00171 base_flags = iface->flags()
00172 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_WORLD)
00173 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN)
00174 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR);
00175 flags_read = true;
00176 }
00177
00178 if (iface->is_visible()) {
00179 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_WORLD) {
00180 have_world = true;
00181
00182 flags |= ObjectPositionInterface::FLAG_HAS_WORLD;
00183 world_x += iface->world_x();
00184 world_y += iface->world_y();
00185 world_z += iface->world_z();
00186 world_num_inputs += 1;
00187
00188 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_EULER_ANGLES) {
00189 roll += iface->roll();
00190 pitch += iface->pitch();
00191 yaw += iface->yaw();
00192 flags |= ObjectPositionInterface::FLAG_HAS_EULER_ANGLES;
00193 euler_num_inputs += 1;
00194 }
00195
00196 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_WORLD_VELOCITY) {
00197 world_x_velocity += iface->world_x_velocity();
00198 world_y_velocity += iface->world_y_velocity();
00199 world_z_velocity += iface->world_z_velocity();
00200 flags |= ObjectPositionInterface::FLAG_HAS_WORLD_VELOCITY;
00201 worldvel_num_inputs += 1;
00202 }
00203 }
00204
00205 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN) {
00206 have_relative = true;
00207
00208 flags |= ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN;
00209
00210 relative_x += iface->relative_x();
00211 relative_y += iface->relative_y();
00212 relative_z += iface->relative_z();
00213 relative_x_velocity += iface->relative_x_velocity();
00214 relative_y_velocity += iface->relative_y_velocity();
00215 relative_z_velocity += iface->relative_z_velocity();
00216 relcart_num_inputs += 1;
00217 }
00218
00219 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR) {
00220 have_relative = true;
00221
00222 flags |= ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR;
00223
00224 distance += iface->distance();
00225 bearing += iface->bearing();
00226 slope += iface->slope();
00227 relpolar_num_inputs += 1;
00228 }
00229
00230 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_EXTENT) {
00231 extent_x += iface->extent_x();
00232 extent_y += iface->extent_y();
00233 extent_z += iface->extent_z();
00234 flags |= ObjectPositionInterface::FLAG_HAS_EXTENT;
00235 extent_num_inputs += 1;
00236 }
00237
00238 if (iface->visibility_history() > vishistory_max) {
00239 vishistory_max = iface->visibility_history();
00240 }
00241 } else {
00242 if (iface->visibility_history() < vishistory_min) {
00243 vishistory_min = iface->visibility_history();
00244 }
00245 }
00246 }
00247 }
00248 }
00249
00250 if ( world_num_inputs > 0 ) {
00251 __output_if->set_world_x(world_x / (float)world_num_inputs);
00252 __output_if->set_world_y(world_y / (float)world_num_inputs);
00253 __output_if->set_world_z(world_z / (float)world_num_inputs);
00254 }
00255 if ( euler_num_inputs > 0 ) {
00256 __output_if->set_roll(roll / (float)euler_num_inputs);
00257 __output_if->set_pitch(pitch / (float)euler_num_inputs);
00258 __output_if->set_yaw(yaw / (float)euler_num_inputs);
00259 }
00260 if ( worldvel_num_inputs > 0) {
00261 __output_if->set_world_x_velocity(world_x_velocity / (float)worldvel_num_inputs);
00262 __output_if->set_world_y_velocity(world_y_velocity / (float)worldvel_num_inputs);
00263 __output_if->set_world_z_velocity(world_z_velocity / (float)worldvel_num_inputs);
00264 }
00265
00266 if ( extent_num_inputs > 0 ) {
00267 __output_if->set_extent_x(extent_x / (float)extent_num_inputs);
00268 __output_if->set_extent_y(extent_y / (float)extent_num_inputs);
00269 __output_if->set_extent_z(extent_z / (float)extent_num_inputs);
00270 }
00271 if ( relcart_num_inputs > 0) {
00272 __output_if->set_relative_x(relative_x / (float)relcart_num_inputs);
00273 __output_if->set_relative_y(relative_y / (float)relcart_num_inputs);
00274 __output_if->set_relative_z(relative_z / (float)relcart_num_inputs);
00275 __output_if->set_relative_x_velocity(relative_x_velocity / (float)relcart_num_inputs);
00276 __output_if->set_relative_y_velocity(relative_y_velocity / (float)relcart_num_inputs);
00277 __output_if->set_relative_z_velocity(relative_z_velocity / (float)relcart_num_inputs);
00278 }
00279 if ( relpolar_num_inputs > 0) {
00280 __output_if->set_distance(distance / (float)relpolar_num_inputs);
00281 __output_if->set_bearing(bearing / (float)relpolar_num_inputs);
00282 __output_if->set_slope(slope / (float)relpolar_num_inputs);
00283 }
00284
00285 visible = have_world || have_relative;
00286
00287 __output_if->set_flags(flags);
00288 __output_if->set_valid(valid);
00289 __output_if->set_visible(visible);
00290 __output_if->set_visibility_history(visible ? vishistory_max : vishistory_min);
00291
00292 __output_if->write();
00293 }