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