playerc_thread.cpp

00001 
00002 /***************************************************************************
00003  *  playerc_thread.cpp - Thread that connects to Player server
00004  *
00005  *  Created: Mon Aug 11 22:45:00 2008
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 #define __STDC_LIMIT_MACROS
00024 
00025 #include "playerc_thread.h"
00026 #include "mapper_factory.h"
00027 
00028 #include <core/exceptions/software.h>
00029 #include <utils/time/time.h>
00030 #include <interfaces/ObjectPositionInterface.h>
00031 
00032 #include <libplayerc++/playerc++.h>
00033 
00034 #include <stdint.h>
00035 
00036 using namespace PlayerCc;
00037 using namespace fawkes;
00038 
00039 /** @class PlayerClientThread "playerc_thread.h"
00040  * Player Client Thread.
00041  * This thread connects to the player server and handles messages
00042  * @author Tim Niemueller
00043  */
00044 
00045 
00046 /** Constructor. */
00047 PlayerClientThread::PlayerClientThread()
00048   : Thread("PlayerClientThread", Thread::OPMODE_WAITFORWAKEUP),
00049     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR)
00050 {
00051   __client = NULL;
00052 }
00053 
00054 
00055 void
00056 PlayerClientThread::init()
00057 {
00058   __client = NULL;
00059 
00060   try {
00061     __cfg_player_host = config->get_string("/player/host");
00062     __cfg_player_port = config->get_uint("/player/port");
00063   } catch (Exception &e) {
00064     e.append("Could not read all required config values for %s", name());
00065     throw;
00066   }
00067 
00068   try {
00069     __client = new PlayerClient(__cfg_player_host.c_str(), __cfg_player_port);
00070 
00071     __client->SetDataMode(PLAYER_DATAMODE_PULL);
00072     __client->SetReplaceRule(/* replace */ true);
00073   } catch (PlayerError &pe) {
00074     finalize();
00075     throw Exception("Failed to connect to Player. Error was '%s'",
00076                     pe.GetErrorStr().c_str());
00077   }
00078 
00079   __client->RequestDeviceList();
00080 
00081   /* shows all available interfaces
00082   std::list<playerc_device_info_t> devices = __client->GetDeviceList();
00083 
00084   for (std::list<playerc_device_info_t>::iterator i = devices.begin(); i != devices.end(); ++i) {
00085     logger->log_debug(name(), "Interface of type %u (%s), index %u, host %u, "
00086                       "robot %u, driver %s",
00087                       i->addr.interf, __client->LookupName(i->addr.interf).c_str(),
00088                       i->addr.index, i->addr.host, i->addr.robot, i->drivername);
00089   }
00090   */
00091 
00092   try {
00093     open_fawkes_interfaces();
00094     open_player_proxies();
00095     create_mappers();
00096   } catch (Exception &e) {
00097     finalize();
00098     throw;
00099   }
00100 }
00101 
00102 void
00103 PlayerClientThread::open_fawkes_interfaces()
00104 {
00105   std::string prefix = "/player/interfaces/fawkes/";
00106   Configuration::ValueIterator *vi = config->search(prefix.c_str());
00107   while (vi->next()) {
00108     if (strcmp(vi->type(), "string") != 0) {
00109       TypeMismatchException e("Only values of type string may occur in %s, "
00110                               "but found value of type %s",
00111                               prefix.c_str(), vi->type());
00112       delete vi;
00113       throw e;
00114     }
00115     std::string uid = vi->get_string();
00116     std::string varname = std::string(vi->path()).substr(prefix.length());
00117     std::string iftype = uid.substr(0, uid.find("::"));
00118     std::string ifname = uid.substr(uid.find("::") + 2);
00119     logger->log_info(name(), "Adding interface %s::%s with name %s writing",
00120                      iftype.c_str(), ifname.c_str(), varname.c_str());
00121     try {
00122       Interface *iface;
00123       iface = blackboard->open_for_writing(iftype.c_str(), ifname.c_str());
00124       __imap[varname] = iface;
00125     } catch (Exception &e) {
00126       delete vi;
00127       throw;
00128     }
00129   }
00130   delete vi;
00131 }
00132 
00133 
00134 void
00135 PlayerClientThread::open_player_proxies()
00136 {
00137   std::list<playerc_device_info_t> devices = __client->GetDeviceList();
00138 
00139   sockaddr_in *addr;
00140   socklen_t    addrlen = sizeof(sockaddr_in);
00141 
00142   if ( ! nnresolver->resolve_name_blocking(__cfg_player_host.c_str(), (sockaddr **)&addr, &addrlen) ) {
00143     throw Exception("Could not lookup IP of %s (player host)", __cfg_player_host.c_str());
00144   }
00145 
00146   unsigned int host  = addr->sin_addr.s_addr;
00147   unsigned int robot = __cfg_player_port;
00148 
00149   std::string prefix = "/player/interfaces/player/";
00150   Configuration::ValueIterator *vi = config->search(prefix.c_str());
00151   while (vi->next()) {
00152     if (strcmp(vi->type(), "string") != 0) {
00153       TypeMismatchException e("Only values of type string may occur in %s, "
00154                               "but found value of type %s",
00155                               prefix.c_str(), vi->type());
00156       delete vi;
00157       throw e;
00158     }
00159     std::string uid = vi->get_string();
00160     std::string varname = std::string(vi->path()).substr(prefix.length());
00161     std::string iftype = uid.substr(0, uid.find(":"));
00162     long int    ifindexl = atol(uid.substr(uid.find(":") + 1).c_str());
00163     if ( ifindexl > (long int)UINT32_MAX ) {
00164       throw Exception("Player interface index is out of range (%li > %u)", ifindexl, UINT32_MAX);
00165     } else if ( ifindexl < 0 ) {
00166       throw Exception("Player interface index is out of range (%li < 0)", ifindexl);
00167     }
00168     unsigned int ifindex = ifindexl;
00169     logger->log_info(name(), "Adding Player interface %s:%u with name %s",
00170                      iftype.c_str(), ifindex, varname.c_str());
00171 
00172     ClientProxy *proxy = NULL;
00173     for (std::list<playerc_device_info_t>::iterator i = devices.begin();
00174          (proxy == NULL) && (i != devices.end()); ++i) {
00175       if ( (i->addr.host == host) &&
00176            (i->addr.robot == robot) &&
00177            (i->addr.index == ifindex) &&
00178            (iftype == __client->LookupName(i->addr.interf)) ) {
00179         // positive match
00180         logger->log_debug(name(), "Opening Player interface of type %u (%s), "
00181                           "index %u, host %u, robot %u, driver %s",
00182                           i->addr.interf, __client->LookupName(i->addr.interf).c_str(),
00183                           i->addr.index, i->addr.host, i->addr.robot, i->drivername);
00184 
00185         if ( iftype == "position2d" ) {
00186           proxy = new Position2dProxy(__client, i->addr.index);
00187         } else if ( iftype == "bumper" ) {
00188           proxy = new BumperProxy(__client, i->addr.index);
00189         } else if ( iftype == "laser" ) {
00190           proxy = new LaserProxy(__client, i->addr.index);
00191         } else {
00192           logger->log_warn(name(), "Unknown interface type %s, ignoring", iftype.c_str());
00193         }
00194       }
00195     }
00196     if ( proxy != NULL ) {
00197       __pmap[varname] = proxy;
00198     } else {
00199       logger->log_warn(name(), "No matching interface found for %s=%s:%u, ignoring",
00200                        varname.c_str(), iftype.c_str(), ifindex);
00201     }
00202   }
00203   delete vi;
00204 }
00205 
00206 
00207 void
00208 PlayerClientThread::create_mappers()
00209 {
00210   for (InterfaceMap::iterator i = __imap.begin(); i != __imap.end(); ++i) {
00211     if ( __pmap.find(i->first) != __pmap.end() ) {
00212       logger->log_debug(name(), "Creating mapping for %s from %s to %s",
00213                         i->first.c_str(), i->second->uid(),
00214                         __pmap[i->first]->GetInterfaceStr().c_str());
00215       __mappers.push_back(PlayerMapperFactory::create_mapper(i->first, i->second,
00216                                                              __pmap[i->first]));
00217     } else {
00218       throw Exception("No matching proxy found for interface %s (%s)",
00219                       i->first.c_str(), i->second->uid());
00220     }
00221   }
00222 
00223   for (ProxyMap::iterator p = __pmap.begin(); p != __pmap.end(); ++p) {
00224     if ( __imap.find(p->first) == __imap.end() ) {
00225       throw Exception("No matching interface found for proxy %s", p->first.c_str());
00226     }
00227   }
00228 }
00229 
00230 
00231 void
00232 PlayerClientThread::finalize()
00233 {
00234   for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
00235     delete *m;
00236   }
00237   __mappers.clear();
00238   
00239   close_fawkes_interfaces();
00240   close_player_proxies();
00241 
00242   delete __client;
00243 }
00244 
00245 
00246 void
00247 PlayerClientThread::close_fawkes_interfaces()
00248 {
00249   for (InterfaceMap::iterator i = __imap.begin(); i != __imap.end(); ++i) {
00250     blackboard->close(i->second);
00251   }
00252   __imap.clear();
00253 }
00254 
00255 
00256 void
00257 PlayerClientThread::close_player_proxies()
00258 {
00259   for (ProxyMap::iterator p = __pmap.begin(); p != __pmap.end(); ++p) {
00260     // dtor is protected, seems to be a Player bug, will discuss upstream
00261     // this is a memleak atm
00262     //delete p->second;
00263   }
00264   __pmap.clear();
00265 }
00266 
00267 
00268 /** Sync Fawkes to player.
00269  * This will call all mappers to sync Fawkes interfaces to Player proxies. This
00270  * is meant to be called by the PlayerF2PThread.
00271  */
00272 void
00273 PlayerClientThread::sync_fawkes_to_player()
00274 {
00275   //logger->log_debug(name(), "Syncing fawkes to player");
00276   try {
00277     for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
00278       (*m)->sync_fawkes_to_player();
00279     }
00280   } catch (PlayerCc::PlayerError &e) {
00281     logger->log_warn(name(), "Failed to update player proxies: %s", e.GetErrorStr().c_str());
00282   }
00283 }
00284 
00285 void
00286 PlayerClientThread::loop()
00287 {
00288   try {
00289     if ( __client->Peek() ) {
00290       __client->Read();
00291 
00292       //logger->log_debug(name(), "Syncing player to fawkes");
00293       for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
00294         (*m)->sync_player_to_fawkes();
00295       }
00296     } else {
00297       //logger->log_warn(name(), "Nothing to sync from player to fawkes");
00298     }
00299   } catch (PlayerCc::PlayerError &e) {
00300     logger->log_warn(name(), "Failed to peek/read data: %s", e.GetErrorStr().c_str());
00301   }
00302 }

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