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
36using namespace PlayerCc;
37using 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
53void
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
99void
100PlayerClientThread::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);
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
134void
135PlayerClientThread::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;
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
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 {
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
217void
218PlayerClientThread::create_mappers()
219{
220 for (InterfaceMap::iterator i = imap_.begin(); i != imap_.end(); ++i) {
221 if (pmap_.find(i->first) != pmap_.end()) {
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
242void
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
256void
257PlayerClientThread::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
265void
266PlayerClientThread::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 */
280void
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
293void
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}
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
void sync_fawkes_to_player()
Sync Fawkes to player.
virtual void loop()
Code to execute in the thread.
PlayerClientThread()
Constructor.
static PlayerProxyFawkesInterfaceMapper * create_mapper(std::string varname, fawkes::Interface *interface, PlayerCc::ClientProxy *proxy)
Create a mapp instance.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Iterator interface to iterate over config values.
Definition: config.h:75
virtual const char * path() const =0
Path of value.
virtual bool next()=0
Check if there is another element and advance to this if possible.
virtual const char * type() const =0
Type of value.
virtual std::string get_string() const =0
Get string value.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
NetworkNameResolver * nnresolver
Network name resolver to lookup IP addresses of hostnames and vice versa.
Definition: network.h:45
bool resolve_name_blocking(const char *name, struct sockaddr **addr, socklen_t *addrlen)
Resolve name and wait for the result.
Definition: resolver.cpp:203
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.