Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * wm_thread.cpp - Fawkes WorldModel Plugin Thread 00004 * 00005 * Created: Fri Jun 29 11:56:48 2007 (on flight to RoboCup 2007, Atlanta) 00006 * Copyright 2006-2008 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 "wm_thread.h" 00024 #include "net_thread.h" 00025 00026 #include "fusers/single_copy.h" 00027 #include "fusers/multi_copy.h" 00028 #include "fusers/objpos_average.h" 00029 #include "fusers/objpos_majority.h" 00030 00031 #include <netcomm/worldinfo/transceiver.h> 00032 #include <utils/system/pathparser.h> 00033 #include <geometry/hom_point.h> 00034 #include <geometry/hom_vector.h> 00035 00036 #include <interfaces/GameStateInterface.h> 00037 #include <interfaces/ObjectPositionInterface.h> 00038 00039 #include <cmath> 00040 #include <cstring> 00041 00042 using namespace std; 00043 using namespace fawkes; 00044 00045 /** @class WorldModelThread <plugins/worldmodel/wm_thread.h> 00046 * Main thread of worldmodel plugin. 00047 * @author Tim Niemueller 00048 * @author Daniel Beck 00049 */ 00050 00051 /** Constructor. 00052 * @param net_thread pointer to a WorldModelNetworkThread 00053 */ 00054 WorldModelThread::WorldModelThread(WorldModelNetworkThread* net_thread) 00055 : Thread("WorldModelThread", Thread::OPMODE_WAITFORWAKEUP), 00056 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE) 00057 { 00058 __net_thread = net_thread; 00059 00060 __wi_send_enabled = false; 00061 __wi_send_interval = 20; 00062 __wi_send_counter = 1; 00063 00064 __wi_send_pose = NULL; 00065 __wi_send_ball = NULL; 00066 00067 00068 __wi_send_interval = 15; 00069 __wi_send_counter = 1; 00070 } 00071 00072 00073 /** Destructor. */ 00074 WorldModelThread::~WorldModelThread() 00075 { 00076 } 00077 00078 void 00079 WorldModelThread::init() 00080 { 00081 try { 00082 __cfg_confspace = config->get_string("/worldmodel/confspace"); 00083 00084 logger->log_debug("WorldModelThread", "Config space: %s", __cfg_confspace.c_str()); 00085 00086 std::string prefix = "/worldmodel/interfaces/" + __cfg_confspace + "/"; 00087 std::list<std::string> combos; 00088 // Read interfaces 00089 Configuration::ValueIterator *vi = config->search(prefix.c_str()); 00090 while (vi->next()) { 00091 if (strcmp(vi->type(), "string") == 0) { 00092 PathParser pp(vi->path()); 00093 if ( pp.size() > 1 ) { 00094 combos.push_back(pp[pp.size() - 2]); 00095 } 00096 } 00097 } 00098 combos.sort(); 00099 combos.unique(); 00100 00101 for (std::list<std::string>::iterator i = combos.begin(); i != combos.end(); ++i) { 00102 std::string type = config->get_string((prefix + *i + "/type").c_str()); 00103 std::string from_id = config->get_string((prefix + *i + "/from_id").c_str()); 00104 std::string to_id = config->get_string((prefix + *i + "/to_id").c_str()); 00105 std::string method = config->get_string((prefix + *i + "/method").c_str()); 00106 00107 if (method == "copy") { 00108 if (from_id.find_first_of("*?[") == std::string::npos) { 00109 logger->log_debug(name(), "Instantiating SingleCopyFuser for %s -> %s (type: %s)", 00110 from_id.c_str(), to_id.c_str(), type.c_str()); 00111 WorldModelSingleCopyFuser *fuser = new WorldModelSingleCopyFuser(blackboard, type.c_str(), 00112 from_id.c_str(), to_id.c_str()); 00113 __fusers.push_back(fuser); 00114 } else { 00115 logger->log_debug(name(), "Instantiating MultiCopyFuser for %s -> %s (type: %s)", 00116 from_id.c_str(), to_id.c_str(), type.c_str()); 00117 WorldModelMultiCopyFuser *fuser = new WorldModelMultiCopyFuser(blackboard, type.c_str(), 00118 from_id.c_str(), to_id.c_str()); 00119 __fusers.push_back(fuser); 00120 } 00121 } else if (method == "average") { 00122 // sanity checks 00123 if (type != "ObjectPositionInterface") { 00124 throw Exception("Can only average interfaces of type ObjectPositionInterface"); 00125 } 00126 logger->log_debug(name(), "Instantiating ObjPosAverageFuser for %s -> %s (type: %s)", 00127 from_id.c_str(), to_id.c_str(), type.c_str()); 00128 WorldModelObjPosAverageFuser *fuser = new WorldModelObjPosAverageFuser(logger, blackboard, 00129 from_id.c_str(), to_id.c_str()); 00130 __fusers.push_back(fuser); 00131 } else if (method == "majority") { 00132 // sanity checks 00133 if (type != "ObjectPositionInterface") { 00134 throw Exception("Can only average interfaces of type ObjectPositionInterface"); 00135 } 00136 std::string own_id = config->get_string((prefix + *i + "/own_id").c_str()); 00137 float self_confidence_radius = config->get_float((prefix + *i + "/confidence_radius").c_str()); 00138 logger->log_debug(name(), "Instantiating MajorityFuser for %s -> %s (type: %s)", 00139 from_id.c_str(), to_id.c_str(), type.c_str()); 00140 WorldModelObjPosMajorityFuser *fuser = new WorldModelObjPosMajorityFuser(logger, blackboard, 00141 own_id.c_str(), 00142 from_id.c_str(), 00143 to_id.c_str(), 00144 self_confidence_radius); 00145 __fusers.push_back(fuser); 00146 } else { 00147 throw Exception("Unknown fuse method '%s', for interface %s -> %s (type %s)", 00148 method.c_str(), from_id.c_str(), to_id.c_str(), type.c_str()); 00149 } 00150 } 00151 00152 } catch (Exception &e) { 00153 e.print_trace(); 00154 } 00155 00156 __wi_send_enabled = false; 00157 try { 00158 std::string prefix = "/worldmodel/wi_send/" + __cfg_confspace + "/"; 00159 __wi_send_enabled = config->get_bool((prefix + "enable_send").c_str()); 00160 00161 if (__wi_send_enabled) { 00162 logger->log_debug(name(), "Sending worldinfo messages enabled"); 00163 00164 std::string pose_id = config->get_string((prefix + "pose_id").c_str()); 00165 std::string ball_id = config->get_string((prefix + "ball_id").c_str()); 00166 00167 logger->log_debug(name(), "Obtaining pose worldinfo data from interface %s.", 00168 pose_id.c_str()); 00169 logger->log_debug(name(), "Obtaining ball worldinfo data from interface %s.", 00170 ball_id.c_str()); 00171 00172 __wi_send_pose = blackboard->open_for_reading<ObjectPositionInterface>(pose_id.c_str()); 00173 __wi_send_ball = blackboard->open_for_reading<ObjectPositionInterface>(ball_id.c_str()); 00174 00175 } else { 00176 logger->log_debug(name(), "Sending worldinfo messages disabled"); 00177 } 00178 00179 } catch (Exception& e) { 00180 if ( __wi_send_enabled) { 00181 throw; 00182 } else { 00183 logger->log_debug(name(), "Sending worldinfo messages disabled (enable not set)"); 00184 } 00185 } 00186 00187 } 00188 00189 00190 /** Clean up when init failed. 00191 * You may only call this from init(). Never ever call it from anywhere 00192 * else! 00193 */ 00194 void 00195 WorldModelThread::init_failure_cleanup() 00196 { 00197 } 00198 00199 00200 void 00201 WorldModelThread::finalize() 00202 { 00203 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) { 00204 delete *__fit; 00205 } 00206 __fusers.clear(); 00207 00208 if (__wi_send_enabled) { 00209 try { 00210 blackboard->close(__wi_send_pose); 00211 blackboard->close(__wi_send_ball); 00212 } catch (Exception& e) { 00213 e.print_trace(); 00214 } 00215 } 00216 } 00217 00218 00219 void 00220 WorldModelThread::loop() 00221 { 00222 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) { 00223 (*__fit)->fuse(); 00224 } 00225 00226 // only send every __wi_send_interval loop 00227 if ( 0 != (__wi_send_counter % __wi_send_interval) ) { 00228 ++__wi_send_counter; 00229 return; 00230 } 00231 00232 __wi_send_counter = 1; 00233 00234 WorldInfoTransceiver* transceiver = __net_thread->get_transceiver(); 00235 00236 if (__wi_send_enabled) { 00237 __wi_send_pose->read(); 00238 __wi_send_ball->read(); 00239 00240 bool do_send = false; 00241 00242 // pose 00243 HomPoint pos; 00244 pos.x( __wi_send_pose->world_x() ); 00245 pos.y( __wi_send_pose->world_y() ); 00246 float yaw = __wi_send_pose->yaw(); 00247 if (__wi_send_pose->has_writer()) { 00248 do_send = true; 00249 transceiver->set_pose(pos.x(), pos.y(), yaw, 00250 __wi_send_pose->world_xyz_covariance() /* TODO */); 00251 transceiver->set_velocity(__wi_send_pose->world_x_velocity(), 00252 __wi_send_pose->world_y_velocity(), 00253 __wi_send_pose->world_z_velocity(), 00254 __wi_send_pose->world_xyz_velocity_covariance()); 00255 00256 // ball 00257 if (__wi_send_ball->has_writer() && __wi_send_ball->is_valid()) { 00258 if (__wi_send_ball->flags() & ObjectPositionInterface::FLAG_HAS_WORLD) { 00259 transceiver->set_glob_ball_pos(__wi_send_ball->world_x(), 00260 __wi_send_ball->world_y(), 00261 __wi_send_ball->world_z(), 00262 __wi_send_ball->world_xyz_covariance() ); 00263 } else { 00264 // compute global ball position 00265 HomVector relative_ball; 00266 relative_ball.x( __wi_send_ball->relative_x() ); 00267 relative_ball.y( __wi_send_ball->relative_y() ); 00268 relative_ball.rotate_z( yaw ); 00269 HomPoint global_ball = pos + relative_ball; 00270 00271 transceiver->set_glob_ball_pos(global_ball.x(), global_ball.y(), 0.0, 00272 __wi_send_ball->dbs_covariance() /* TODO */); 00273 } 00274 transceiver->set_glob_ball_visible(__wi_send_ball->is_visible(), 00275 __wi_send_ball->visibility_history()); 00276 00277 // TODO 00278 // transceiver->set_glob_ball_velocity(__wi_send_ball->relative_x_velocity(), 00279 // __wi_send_ball->relative_y_velocity(), 00280 // __wi_send_ball->relative_z_velocity(), 00281 // __wi_send_ball->relative_xyz_velocity_covariance()); 00282 } 00283 } 00284 00285 if (do_send) { 00286 transceiver->send(); 00287 } 00288 } 00289 }