23 #define __STDC_LIMIT_MACROS 25 #include "playerc_thread.h" 26 #include "mapper_factory.h" 28 #include <core/exceptions/software.h> 29 #include <utils/time/time.h> 30 #include <interfaces/ObjectPositionInterface.h> 32 #include <libplayerc++/playerc++.h> 48 :
Thread(
"PlayerClientThread",
Thread::OPMODE_WAITFORWAKEUP),
64 e.
append(
"Could not read all required config values for %s",
name());
69 __client =
new PlayerClient(__cfg_player_host.c_str(), __cfg_player_port);
71 __client->SetDataMode(PLAYER_DATAMODE_PULL);
72 __client->SetReplaceRule(
true);
73 }
catch (PlayerError &pe) {
75 throw Exception(
"Failed to connect to Player. Error was '%s'",
76 pe.GetErrorStr().c_str());
79 __client->RequestDeviceList();
93 open_fawkes_interfaces();
94 open_player_proxies();
103 PlayerClientThread::open_fawkes_interfaces()
105 std::string prefix =
"/player/interfaces/fawkes/";
108 if (strcmp(vi->
type(),
"string") != 0) {
110 "but found value of type %s",
111 prefix.c_str(), vi->
type());
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);
120 iftype.c_str(), ifname.c_str(), varname.c_str());
124 __imap[varname] = iface;
135 PlayerClientThread::open_player_proxies()
137 std::list<playerc_device_info_t> devices = __client->GetDeviceList();
140 socklen_t addrlen =
sizeof(sockaddr_in);
143 throw Exception(
"Could not lookup IP of %s (player host)", __cfg_player_host.c_str());
146 unsigned int host = addr->sin_addr.s_addr;
147 unsigned int robot = __cfg_player_port;
149 std::string prefix =
"/player/interfaces/player/";
152 if (strcmp(vi->
type(),
"string") != 0) {
154 "but found value of type %s",
155 prefix.c_str(), vi->
type());
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);
168 unsigned int ifindex = ifindexl;
170 iftype.c_str(), ifindex, varname.c_str());
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)) ) {
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);
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);
196 if ( proxy != NULL ) {
197 __pmap[varname] = proxy;
200 varname.c_str(), iftype.c_str(), ifindex);
208 PlayerClientThread::create_mappers()
210 for (InterfaceMap::iterator i = __imap.begin(); i != __imap.end(); ++i) {
211 if ( __pmap.find(i->first) != __pmap.end() ) {
213 i->first.c_str(), i->second->uid(),
214 __pmap[i->first]->GetInterfaceStr().c_str());
218 throw Exception(
"No matching proxy found for interface %s (%s)",
219 i->first.c_str(), i->second->uid());
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());
234 for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
239 close_fawkes_interfaces();
240 close_player_proxies();
247 PlayerClientThread::close_fawkes_interfaces()
249 for (InterfaceMap::iterator i = __imap.begin(); i != __imap.end(); ++i) {
257 PlayerClientThread::close_player_proxies()
259 for (ProxyMap::iterator p = __pmap.begin(); p != __pmap.end(); ++p) {
277 for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
278 (*m)->sync_fawkes_to_player();
280 }
catch (PlayerCc::PlayerError &e) {
281 logger->
log_warn(
name(),
"Failed to update player proxies: %s", e.GetErrorStr().c_str());
289 if ( __client->Peek() ) {
293 for (MapperList::iterator m = __mappers.begin(); m != __mappers.end(); ++m) {
294 (*m)->sync_player_to_fawkes();
299 }
catch (PlayerCc::PlayerError &e) {
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.
Base class for all Fawkes BlackBoard interfaces.
Logger * logger
This is the Logger member used to access the logger.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
NetworkNameResolver * nnresolver
Network name resolver to lookup IP addresses of hostnames and vice versa.
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.
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.
bool resolve_name_blocking(const char *name, struct sockaddr **addr, socklen_t *addrlen)
Resolve name and wait for the result.
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.
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.
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.
virtual void close(Interface *interface)=0
Close interface.