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