Fawkes API Fawkes Development Version
|
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 }