Fawkes API  Fawkes Development Version
playerc_thread.cpp
1 
2 /***************************************************************************
3  * playerc_thread.cpp - Thread that connects to Player server
4  *
5  * Created: Mon Aug 11 22:45:00 2008
6  * Copyright 2006-2008 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #define __STDC_LIMIT_MACROS
24 
25 #include "playerc_thread.h"
26 #include "mapper_factory.h"
27 
28 #include <core/exceptions/software.h>
29 #include <utils/time/time.h>
30 #include <interfaces/ObjectPositionInterface.h>
31 
32 #include <libplayerc++/playerc++.h>
33 
34 #include <stdint.h>
35 
36 using namespace PlayerCc;
37 using namespace fawkes;
38 
39 /** @class PlayerClientThread "playerc_thread.h"
40  * Player Client Thread.
41  * This thread connects to the player server and handles messages
42  * @author Tim Niemueller
43  */
44 
45 
46 /** Constructor. */
48  : Thread("PlayerClientThread", Thread::OPMODE_WAITFORWAKEUP),
49  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
50 {
51  __client = NULL;
52 }
53 
54 
55 void
57 {
58  __client = NULL;
59 
60  try {
61  __cfg_player_host = config->get_string("/player/host");
62  __cfg_player_port = config->get_uint("/player/port");
63  } catch (Exception &e) {
64  e.append("Could not read all required config values for %s", name());
65  throw;
66  }
67 
68  try {
69  __client = new PlayerClient(__cfg_player_host.c_str(), __cfg_player_port);
70 
71  __client->SetDataMode(PLAYER_DATAMODE_PULL);
72  __client->SetReplaceRule(/* replace */ true);
73  } catch (PlayerError &pe) {
74  finalize();
75  throw Exception("Failed to connect to Player. Error was '%s'",
76  pe.GetErrorStr().c_str());
77  }
78 
79  __client->RequestDeviceList();
80 
81  /* shows all available interfaces
82  std::list<playerc_device_info_t> devices = __client->GetDeviceList();
83 
84  for (std::list<playerc_device_info_t>::iterator i = devices.begin(); i != devices.end(); ++i) {
85  logger->log_debug(name(), "Interface of type %u (%s), index %u, host %u, "
86  "robot %u, driver %s",
87  i->addr.interf, __client->LookupName(i->addr.interf).c_str(),
88  i->addr.index, i->addr.host, i->addr.robot, i->drivername);
89  }
90  */
91 
92  try {
93  open_fawkes_interfaces();
94  open_player_proxies();
95  create_mappers();
96  } catch (Exception &e) {
97  finalize();
98  throw;
99  }
100 }
101 
102 void
103 PlayerClientThread::open_fawkes_interfaces()
104 {
105  std::string prefix = "/player/interfaces/fawkes/";
106  Configuration::ValueIterator *vi = config->search(prefix.c_str());
107  while (vi->next()) {
108  if (strcmp(vi->type(), "string") != 0) {
109  TypeMismatchException e("Only values of type string may occur in %s, "
110  "but found value of type %s",
111  prefix.c_str(), vi->type());
112  delete vi;
113  throw e;
114  }
115  std::string uid = vi->get_string();
116  std::string varname = std::string(vi->path()).substr(prefix.length());
117  std::string iftype = uid.substr(0, uid.find("::"));
118  std::string ifname = uid.substr(uid.find("::") + 2);
119  logger->log_info(name(), "Adding interface %s::%s with name %s writing",
120  iftype.c_str(), ifname.c_str(), varname.c_str());
121  try {
122  Interface *iface;
123  iface = blackboard->open_for_writing(iftype.c_str(), ifname.c_str());
124  __imap[varname] = iface;
125  } catch (Exception &e) {
126  delete vi;
127  throw;
128  }
129  }
130  delete vi;
131 }
132 
133 
134 void
135 PlayerClientThread::open_player_proxies()
136 {
137  std::list<playerc_device_info_t> devices = __client->GetDeviceList();
138 
139  sockaddr_in *addr;
140  socklen_t addrlen = sizeof(sockaddr_in);
141 
142  if ( ! nnresolver->resolve_name_blocking(__cfg_player_host.c_str(), (sockaddr **)&addr, &addrlen) ) {
143  throw Exception("Could not lookup IP of %s (player host)", __cfg_player_host.c_str());
144  }
145 
146  unsigned int host = addr->sin_addr.s_addr;
147  unsigned int robot = __cfg_player_port;
148 
149  std::string prefix = "/player/interfaces/player/";
150  Configuration::ValueIterator *vi = config->search(prefix.c_str());
151  while (vi->next()) {
152  if (strcmp(vi->type(), "string") != 0) {
153  TypeMismatchException e("Only values of type string may occur in %s, "
154  "but found value of type %s",
155  prefix.c_str(), vi->type());
156  delete vi;
157  throw e;
158  }
159  std::string uid = vi->get_string();
160  std::string varname = std::string(vi->path()).substr(prefix.length());
161  std::string iftype = uid.substr(0, uid.find(":"));
162  long int ifindexl = atol(uid.substr(uid.find(":") + 1).c_str());
163  if ( ifindexl > (long int)UINT32_MAX ) {
164  throw Exception("Player interface index is out of range (%li > %u)", ifindexl, UINT32_MAX);
165  } else if ( ifindexl < 0 ) {
166  throw Exception("Player interface index is out of range (%li < 0)", ifindexl);
167  }
168  unsigned int ifindex = ifindexl;
169  logger->log_info(name(), "Adding Player interface %s:%u with name %s",
170  iftype.c_str(), ifindex, varname.c_str());
171 
172  ClientProxy *proxy = NULL;
173  for (std::list<playerc_device_info_t>::iterator i = devices.begin();
174  (proxy == NULL) && (i != devices.end()); ++i) {
175  if ( (i->addr.host == host) &&
176  (i->addr.robot == robot) &&
177  (i->addr.index == ifindex) &&
178  (iftype == __client->LookupName(i->addr.interf)) ) {
179  // positive match
180  logger->log_debug(name(), "Opening Player interface of type %u (%s), "
181  "index %u, host %u, robot %u, driver %s",
182  i->addr.interf, __client->LookupName(i->addr.interf).c_str(),
183  i->addr.index, i->addr.host, i->addr.robot, i->drivername);
184 
185  if ( iftype == "position2d" ) {
186  proxy = new Position2dProxy(__client, i->addr.index);
187  } else if ( iftype == "bumper" ) {
188  proxy = new BumperProxy(__client, i->addr.index);
189  } else if ( iftype == "laser" ) {
190  proxy = new LaserProxy(__client, i->addr.index);
191  } else {
192  logger->log_warn(name(), "Unknown interface type %s, ignoring", iftype.c_str());
193  }
194  }
195  }
196  if ( proxy != NULL ) {
197  __pmap[varname] = proxy;
198  } else {
199  logger->log_warn(name(), "No matching interface found for %s=%s:%u, ignoring",
200  varname.c_str(), iftype.c_str(), ifindex);
201  }
202  }
203  delete vi;
204 }
205 
206 
207 void
208 PlayerClientThread::create_mappers()
209 {
210  for (InterfaceMap::iterator i = __imap.begin(); i != __imap.end(); ++i) {
211  if ( __pmap.find(i->first) != __pmap.end() ) {
212  logger->log_debug(name(), "Creating mapping for %s from %s to %s",
213  i->first.c_str(), i->second->uid(),
214  __pmap[i->first]->GetInterfaceStr().c_str());
215  __mappers.push_back(PlayerMapperFactory::create_mapper(i->first, i->second,
216  __pmap[i->first]));
217  } else {
218  throw Exception("No matching proxy found for interface %s (%s)",
219  i->first.c_str(), i->second->uid());
220  }
221  }
222 
223  for (ProxyMap::iterator p = __pmap.begin(); p != __pmap.end(); ++p) {
224  if ( __imap.find(p->first) == __imap.end() ) {
225  throw Exception("No matching interface found for proxy %s", p->first.c_str());
226  }
227  }
228 }
229 
230 
231 void
233 {
234  for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
235  delete *m;
236  }
237  __mappers.clear();
238 
239  close_fawkes_interfaces();
240  close_player_proxies();
241 
242  delete __client;
243 }
244 
245 
246 void
247 PlayerClientThread::close_fawkes_interfaces()
248 {
249  for (InterfaceMap::iterator i = __imap.begin(); i != __imap.end(); ++i) {
250  blackboard->close(i->second);
251  }
252  __imap.clear();
253 }
254 
255 
256 void
257 PlayerClientThread::close_player_proxies()
258 {
259  for (ProxyMap::iterator p = __pmap.begin(); p != __pmap.end(); ++p) {
260  // dtor is protected, seems to be a Player bug, will discuss upstream
261  // this is a memleak atm
262  //delete p->second;
263  }
264  __pmap.clear();
265 }
266 
267 
268 /** Sync Fawkes to player.
269  * This will call all mappers to sync Fawkes interfaces to Player proxies. This
270  * is meant to be called by the PlayerF2PThread.
271  */
272 void
274 {
275  //logger->log_debug(name(), "Syncing fawkes to player");
276  try {
277  for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
278  (*m)->sync_fawkes_to_player();
279  }
280  } catch (PlayerCc::PlayerError &e) {
281  logger->log_warn(name(), "Failed to update player proxies: %s", e.GetErrorStr().c_str());
282  }
283 }
284 
285 void
287 {
288  try {
289  if ( __client->Peek() ) {
290  __client->Read();
291 
292  //logger->log_debug(name(), "Syncing player to fawkes");
293  for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
294  (*m)->sync_player_to_fawkes();
295  }
296  } else {
297  //logger->log_warn(name(), "Nothing to sync from player to fawkes");
298  }
299  } catch (PlayerCc::PlayerError &e) {
300  logger->log_warn(name(), "Failed to peek/read data: %s", e.GetErrorStr().c_str());
301  }
302 }
virtual void loop()
Code to execute in the thread.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void sync_fawkes_to_player()
Sync Fawkes to player.
virtual const char * type() const =0
Type of value.
Fawkes library namespace.
virtual void init()
Initialize the thread.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
NetworkNameResolver * nnresolver
Network name resolver to lookup IP addresses of hostnames and vice versa.
Definition: network.h:48
static PlayerProxyFawkesInterfaceMapper * create_mapper(std::string varname, fawkes::Interface *interface, PlayerCc::ClientProxy *proxy)
Create a mapp instance.
virtual std::string get_string() const =0
Get string value.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual const char * path() const =0
Path of value.
PlayerClientThread()
Constructor.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Iterator interface to iterate over config values.
Definition: config.h:72
bool resolve_name_blocking(const char *name, struct sockaddr **addr, socklen_t *addrlen)
Resolve name and wait for the result.
Definition: resolver.cpp:213
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual void finalize()
Finalize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:341
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.