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 
27 #include "mapper_factory.h"
28 
29 #include <core/exceptions/software.h>
30 #include <interfaces/ObjectPositionInterface.h>
31 #include <libplayerc++/playerc++.h>
32 #include <utils/time/time.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 /** Constructor. */
47 : Thread("PlayerClientThread", Thread::OPMODE_WAITFORWAKEUP),
48  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
49 {
50  client_ = NULL;
51 }
52 
53 void
55 {
56  client_ = NULL;
57 
58  try {
59  cfg_player_host_ = config->get_string("/player/host");
60  cfg_player_port_ = config->get_uint("/player/port");
61  } catch (Exception &e) {
62  e.append("Could not read all required config values for %s", name());
63  throw;
64  }
65 
66  try {
67  client_ = new PlayerClient(cfg_player_host_.c_str(), cfg_player_port_);
68 
69  client_->SetDataMode(PLAYER_DATAMODE_PULL);
70  client_->SetReplaceRule(/* replace */ true);
71  } catch (PlayerError &pe) {
72  finalize();
73  throw Exception("Failed to connect to Player. Error was '%s'", pe.GetErrorStr().c_str());
74  }
75 
76  client_->RequestDeviceList();
77 
78  /* shows all available interfaces
79  std::list<playerc_device_info_t> devices = client_->GetDeviceList();
80 
81  for (std::list<playerc_device_info_t>::iterator i = devices.begin(); i != devices.end(); ++i) {
82  logger->log_debug(name(), "Interface of type %u (%s), index %u, host %u, "
83  "robot %u, driver %s",
84  i->addr.interf, client_->LookupName(i->addr.interf).c_str(),
85  i->addr.index, i->addr.host, i->addr.robot, i->drivername);
86  }
87  */
88 
89  try {
90  open_fawkes_interfaces();
91  open_player_proxies();
92  create_mappers();
93  } catch (Exception &e) {
94  finalize();
95  throw;
96  }
97 }
98 
99 void
100 PlayerClientThread::open_fawkes_interfaces()
101 {
102  std::string prefix = "/player/interfaces/fawkes/";
103  Configuration::ValueIterator *vi = config->search(prefix.c_str());
104  while (vi->next()) {
105  if (strcmp(vi->type(), "string") != 0) {
106  TypeMismatchException e("Only values of type string may occur in %s, "
107  "but found value of type %s",
108  prefix.c_str(),
109  vi->type());
110  delete vi;
111  throw e;
112  }
113  std::string uid = vi->get_string();
114  std::string varname = std::string(vi->path()).substr(prefix.length());
115  std::string iftype = uid.substr(0, uid.find("::"));
116  std::string ifname = uid.substr(uid.find("::") + 2);
117  logger->log_info(name(),
118  "Adding interface %s::%s with name %s writing",
119  iftype.c_str(),
120  ifname.c_str(),
121  varname.c_str());
122  try {
123  Interface *iface;
124  iface = blackboard->open_for_writing(iftype.c_str(), ifname.c_str());
125  imap_[varname] = iface;
126  } catch (Exception &e) {
127  delete vi;
128  throw;
129  }
130  }
131  delete vi;
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(),
156  vi->type());
157  delete vi;
158  throw e;
159  }
160  std::string uid = vi->get_string();
161  std::string varname = std::string(vi->path()).substr(prefix.length());
162  std::string iftype = uid.substr(0, uid.find(":"));
163  long int ifindexl = atol(uid.substr(uid.find(":") + 1).c_str());
164  if (ifindexl > (long int)UINT32_MAX) {
165  throw Exception("Player interface index is out of range (%li > %u)", ifindexl, UINT32_MAX);
166  } else if (ifindexl < 0) {
167  throw Exception("Player interface index is out of range (%li < 0)", ifindexl);
168  }
169  unsigned int ifindex = ifindexl;
170  logger->log_info(name(),
171  "Adding Player interface %s:%u with name %s",
172  iftype.c_str(),
173  ifindex,
174  varname.c_str());
175 
176  ClientProxy *proxy = NULL;
177  for (std::list<playerc_device_info_t>::iterator i = devices.begin();
178  (proxy == NULL) && (i != devices.end());
179  ++i) {
180  if ((i->addr.host == host) && (i->addr.robot == robot) && (i->addr.index == ifindex)
181  && (iftype == client_->LookupName(i->addr.interf))) {
182  // positive match
183  logger->log_debug(name(),
184  "Opening Player interface of type %u (%s), "
185  "index %u, host %u, robot %u, driver %s",
186  i->addr.interf,
187  client_->LookupName(i->addr.interf).c_str(),
188  i->addr.index,
189  i->addr.host,
190  i->addr.robot,
191  i->drivername);
192 
193  if (iftype == "position2d") {
194  proxy = new Position2dProxy(client_, i->addr.index);
195  } else if (iftype == "bumper") {
196  proxy = new BumperProxy(client_, i->addr.index);
197  } else if (iftype == "laser") {
198  proxy = new LaserProxy(client_, i->addr.index);
199  } else {
200  logger->log_warn(name(), "Unknown interface type %s, ignoring", iftype.c_str());
201  }
202  }
203  }
204  if (proxy != NULL) {
205  pmap_[varname] = proxy;
206  } else {
207  logger->log_warn(name(),
208  "No matching interface found for %s=%s:%u, ignoring",
209  varname.c_str(),
210  iftype.c_str(),
211  ifindex);
212  }
213  }
214  delete vi;
215 }
216 
217 void
218 PlayerClientThread::create_mappers()
219 {
220  for (InterfaceMap::iterator i = imap_.begin(); i != imap_.end(); ++i) {
221  if (pmap_.find(i->first) != pmap_.end()) {
222  logger->log_debug(name(),
223  "Creating mapping for %s from %s to %s",
224  i->first.c_str(),
225  i->second->uid(),
226  pmap_[i->first]->GetInterfaceStr().c_str());
227  mappers_.push_back(PlayerMapperFactory::create_mapper(i->first, i->second, pmap_[i->first]));
228  } else {
229  throw Exception("No matching proxy found for interface %s (%s)",
230  i->first.c_str(),
231  i->second->uid());
232  }
233  }
234 
235  for (ProxyMap::iterator p = pmap_.begin(); p != pmap_.end(); ++p) {
236  if (imap_.find(p->first) == imap_.end()) {
237  throw Exception("No matching interface found for proxy %s", p->first.c_str());
238  }
239  }
240 }
241 
242 void
244 {
245  for (MapperList::iterator m = mappers_.begin(); m != mappers_.end(); ++m) {
246  delete *m;
247  }
248  mappers_.clear();
249 
250  close_fawkes_interfaces();
251  close_player_proxies();
252 
253  delete client_;
254 }
255 
256 void
257 PlayerClientThread::close_fawkes_interfaces()
258 {
259  for (InterfaceMap::iterator i = imap_.begin(); i != imap_.end(); ++i) {
260  blackboard->close(i->second);
261  }
262  imap_.clear();
263 }
264 
265 void
266 PlayerClientThread::close_player_proxies()
267 {
268  for (ProxyMap::iterator p = pmap_.begin(); p != pmap_.end(); ++p) {
269  // dtor is protected, seems to be a Player bug, will discuss upstream
270  // this is a memleak atm
271  //delete p->second;
272  }
273  pmap_.clear();
274 }
275 
276 /** Sync Fawkes to player.
277  * This will call all mappers to sync Fawkes interfaces to Player proxies. This
278  * is meant to be called by the PlayerF2PThread.
279  */
280 void
282 {
283  //logger->log_debug(name(), "Syncing fawkes to player");
284  try {
285  for (MapperList::iterator m = mappers_.begin(); m != mappers_.end(); ++m) {
286  (*m)->sync_fawkes_to_player();
287  }
288  } catch (PlayerCc::PlayerError &e) {
289  logger->log_warn(name(), "Failed to update player proxies: %s", e.GetErrorStr().c_str());
290  }
291 }
292 
293 void
295 {
296  try {
297  if (client_->Peek()) {
298  client_->Read();
299 
300  //logger->log_debug(name(), "Syncing player to fawkes");
301  for (MapperList::iterator m = mappers_.begin(); m != mappers_.end(); ++m) {
302  (*m)->sync_player_to_fawkes();
303  }
304  } else {
305  //logger->log_warn(name(), "Nothing to sync from player to fawkes");
306  }
307  } catch (PlayerCc::PlayerError &e) {
308  logger->log_warn(name(), "Failed to peek/read data: %s", e.GetErrorStr().c_str());
309  }
310 }
PlayerClientThread::PlayerClientThread
PlayerClientThread()
Constructor.
Definition: playerc_thread.cpp:46
PlayerClientThread::loop
virtual void loop()
Code to execute in the thread.
Definition: playerc_thread.cpp:294
PlayerClientThread::finalize
virtual void finalize()
Finalize the thread.
Definition: playerc_thread.cpp:243
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::Configuration::ValueIterator::get_string
virtual std::string get_string() const =0
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
PlayerMapperFactory::create_mapper
static PlayerProxyFawkesInterfaceMapper * create_mapper(std::string varname, fawkes::Interface *interface, PlayerCc::ClientProxy *proxy)
Create a mapp instance.
Definition: mapper_factory.cpp:54
fawkes::Configuration::ValueIterator
Definition: config.h:77
fawkes::Exception::append
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
fawkes::Configuration::search
virtual ValueIterator * search(const char *path)=0
fawkes::NetworkAspect::nnresolver
NetworkNameResolver * nnresolver
Definition: network.h:57
fawkes::TypeMismatchException
Definition: software.h:49
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::Interface
Definition: interface.h:78
PlayerClientThread::init
virtual void init()
Initialize the thread.
Definition: playerc_thread.cpp:54
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Configuration::ValueIterator::type
virtual const char * type() const =0
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
PlayerClientThread::sync_fawkes_to_player
void sync_fawkes_to_player()
Sync Fawkes to player.
Definition: playerc_thread.cpp:281
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Configuration::ValueIterator::path
virtual const char * path() const =0
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Configuration::ValueIterator::next
virtual bool next()=0
fawkes::NetworkNameResolver::resolve_name_blocking
bool resolve_name_blocking(const char *name, struct sockaddr **addr, socklen_t *addrlen)
Resolve name and wait for the result.
Definition: resolver.cpp:209
fawkes::Exception
Definition: exception.h:41