objpos_average.cpp

00001 
00002 /***************************************************************************
00003  *  objpos_average.cpp - Fawkes WorldModel Object Position Average Fuser
00004  *
00005  *  Created: Tue Jan 13 13:51:45 2009
00006  *  Copyright  2006-2009  Tim Niemueller [www.niemueller.de]
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 #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 /** @class WorldModelObjPosAverageFuser "objpos_average.h"
00035  * ObjectPositionModel average fuser.
00036  * This fuser takes a number of ObjectPositionInterface instanced and fuses them
00037  * into a single ObjectPositionInterface by averaging over the source interfaces.
00038  * It registers as an observer and opens any newly created interface that matches
00039  * the ID pattern.
00040  * @author Tim Niemueller
00041  */
00042 
00043 /** Constructor.
00044  * @param blackboard BlackBoard
00045  * @param from_id_pattern pattern for ID of the interfaces to copy from
00046  * @param to_id ID of the interface to copy to
00047  * @param logger logger
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     // If our output interface was already opened open_multiple might have opened
00065     // it as well, check and close if that was the case
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 /** Destructor. */
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 }

Generated on Tue Feb 22 13:32:31 2011 for Fawkes API by  doxygen 1.4.7