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 __blackboard->close(from_if);
00116 e.print_trace();
00117 }
00118 }
00119
00120
00121 void
00122 WorldModelObjPosAverageFuser::fuse()
00123 {
00124 MutexLocker lock(__input_ifs.mutex());
00125
00126 unsigned int flags = 0;
00127 unsigned int base_flags = 0;
00128 unsigned int world_num_inputs=0, extent_num_inputs=0, euler_num_inputs = 0,
00129 worldvel_num_inputs = 0, relcart_num_inputs = 0, relpolar_num_inputs = 0;
00130 float roll = 0, pitch = 0, yaw = 0, distance = 0, bearing = 0, slope = 0,
00131 world_x = 0, world_y = 0, world_z = 0,
00132 relative_x = 0, relative_y = 0, relative_z = 0,
00133 extent_x = 0, extent_y = 0, extent_z = 0,
00134 world_x_velocity = 0, world_y_velocity = 0, world_z_velocity = 0,
00135 relative_x_velocity = 0, relative_y_velocity = 0, relative_z_velocity = 0;
00136 bool valid = true, visible = true;
00137 int vishistory_min = 0, vishistory_max = 0;
00138 unsigned int object_type = 0;
00139 bool object_type_warned = false;
00140 bool flags_read = false;
00141 bool have_world = false, have_relative = false;
00142
00143 for (__iii = __input_ifs.begin(); __iii != __input_ifs.end(); ++__iii) {
00144 ObjectPositionInterface *iface = *__iii;
00145 if (iface->has_writer()) {
00146 iface->read();
00147 if (iface->is_valid()) {
00148 if ( (object_type != 0) && (iface->object_type() != object_type) &&
00149 ! object_type_warned) {
00150 __logger->log_warn("WMObjPosAvgFus", "Object types of input interfaces "
00151 "for %s disagree, %s has %u, expected was %u",
00152 __to_id.c_str(), iface->uid(), iface->object_type(),
00153 object_type);
00154 object_type_warned = true;
00155 } else {
00156 object_type = iface->object_type();
00157 }
00158
00159 if (flags_read) {
00160 unsigned int iflags = iface->flags()
00161 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_WORLD)
00162 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN)
00163 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR);
00164 if (iflags != base_flags) {
00165 __logger->log_warn("WMObjPosAvgFus", "Interface flags for %s "
00166 "disagree. Exected %x, got %x", base_flags, iflags);
00167 }
00168 } else {
00169 base_flags = iface->flags()
00170 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_WORLD)
00171 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN)
00172 & (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR);
00173 flags_read = true;
00174 }
00175
00176 if (iface->is_visible()) {
00177 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_WORLD) {
00178 have_world = true;
00179
00180 flags |= ObjectPositionInterface::FLAG_HAS_WORLD;
00181 world_x += iface->world_x();
00182 world_y += iface->world_y();
00183 world_z += iface->world_z();
00184 world_num_inputs += 1;
00185
00186 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_EULER_ANGLES) {
00187 roll += iface->roll();
00188 pitch += iface->pitch();
00189 yaw += iface->yaw();
00190 flags |= ObjectPositionInterface::FLAG_HAS_EULER_ANGLES;
00191 euler_num_inputs += 1;
00192 }
00193
00194 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_WORLD_VELOCITY) {
00195 world_x_velocity += iface->world_x_velocity();
00196 world_y_velocity += iface->world_y_velocity();
00197 world_z_velocity += iface->world_z_velocity();
00198 flags |= ObjectPositionInterface::FLAG_HAS_WORLD_VELOCITY;
00199 worldvel_num_inputs += 1;
00200 }
00201 }
00202
00203 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN) {
00204 have_relative = true;
00205
00206 flags |= ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN;
00207
00208 relative_x += iface->relative_x();
00209 relative_y += iface->relative_y();
00210 relative_z += iface->relative_z();
00211 relative_x_velocity += iface->relative_x_velocity();
00212 relative_y_velocity += iface->relative_y_velocity();
00213 relative_z_velocity += iface->relative_z_velocity();
00214 relcart_num_inputs += 1;
00215 }
00216
00217 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR) {
00218 have_relative = true;
00219
00220 flags |= ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR;
00221
00222 distance += iface->distance();
00223 bearing += iface->bearing();
00224 slope += iface->slope();
00225 relpolar_num_inputs += 1;
00226 }
00227
00228 if (iface->flags() & ObjectPositionInterface::FLAG_HAS_EXTENT) {
00229 extent_x += iface->extent_x();
00230 extent_y += iface->extent_y();
00231 extent_z += iface->extent_z();
00232 flags |= ObjectPositionInterface::FLAG_HAS_EXTENT;
00233 extent_num_inputs += 1;
00234 }
00235
00236 if (iface->visibility_history() > vishistory_max) {
00237 vishistory_max = iface->visibility_history();
00238 }
00239 } else {
00240 if (iface->visibility_history() < vishistory_min) {
00241 vishistory_min = iface->visibility_history();
00242 }
00243 }
00244 }
00245 }
00246 }
00247
00248 if ( world_num_inputs > 0 ) {
00249 __output_if->set_world_x(world_x / (float)world_num_inputs);
00250 __output_if->set_world_y(world_y / (float)world_num_inputs);
00251 __output_if->set_world_z(world_z / (float)world_num_inputs);
00252 }
00253 if ( euler_num_inputs > 0 ) {
00254 __output_if->set_roll(roll / (float)euler_num_inputs);
00255 __output_if->set_pitch(pitch / (float)euler_num_inputs);
00256 __output_if->set_yaw(yaw / (float)euler_num_inputs);
00257 }
00258 if ( worldvel_num_inputs > 0) {
00259 __output_if->set_world_x_velocity(world_x_velocity / (float)worldvel_num_inputs);
00260 __output_if->set_world_y_velocity(world_y_velocity / (float)worldvel_num_inputs);
00261 __output_if->set_world_z_velocity(world_z_velocity / (float)worldvel_num_inputs);
00262 }
00263
00264 if ( extent_num_inputs > 0 ) {
00265 __output_if->set_extent_x(extent_x / (float)extent_num_inputs);
00266 __output_if->set_extent_y(extent_y / (float)extent_num_inputs);
00267 __output_if->set_extent_z(extent_z / (float)extent_num_inputs);
00268 }
00269 if ( relcart_num_inputs > 0) {
00270 __output_if->set_relative_x(relative_x / (float)relcart_num_inputs);
00271 __output_if->set_relative_y(relative_y / (float)relcart_num_inputs);
00272 __output_if->set_relative_z(relative_z / (float)relcart_num_inputs);
00273 __output_if->set_relative_x_velocity(relative_x_velocity / (float)relcart_num_inputs);
00274 __output_if->set_relative_y_velocity(relative_y_velocity / (float)relcart_num_inputs);
00275 __output_if->set_relative_z_velocity(relative_z_velocity / (float)relcart_num_inputs);
00276 }
00277 if ( relpolar_num_inputs > 0) {
00278 __output_if->set_distance(distance / (float)relpolar_num_inputs);
00279 __output_if->set_bearing(bearing / (float)relpolar_num_inputs);
00280 __output_if->set_slope(slope / (float)relpolar_num_inputs);
00281 }
00282
00283 visible = have_world || have_relative;
00284
00285 __output_if->set_flags(flags);
00286 __output_if->set_valid(valid);
00287 __output_if->set_visible(visible);
00288 __output_if->set_visibility_history(visible ? vishistory_max : vishistory_min);
00289
00290 __output_if->write();
00291 }
