00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00040
00041
00042
00043
00044
00045
00046
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( 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
00082
00083
00084
00085
00086
00087
00088
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
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
00261
00262
00263 }
00264 __pmap.clear();
00265 }
00266
00267
00268
00269
00270
00271
00272 void
00273 PlayerClientThread::sync_fawkes_to_player()
00274 {
00275
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
00293 for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
00294 (*m)->sync_player_to_fawkes();
00295 }
00296 } else {
00297
00298 }
00299 } catch (PlayerCc::PlayerError &e) {
00300 logger->log_warn(name(), "Failed to peek/read data: %s", e.GetErrorStr().c_str());
00301 }
00302 }