23 #define __STDC_LIMIT_MACROS
25 #include "playerc_thread.h"
27 #include "mapper_factory.h"
29 #include <core/exceptions/software.h>
30 #include <interfaces/ObjectPositionInterface.h>
31 #include <libplayerc++/playerc++.h>
32 #include <utils/time/time.h>
36 using namespace PlayerCc;
47 :
Thread(
"PlayerClientThread",
Thread::OPMODE_WAITFORWAKEUP),
62 e.
append(
"Could not read all required config values for %s",
name());
67 client_ =
new PlayerClient(cfg_player_host_.c_str(), cfg_player_port_);
69 client_->SetDataMode(PLAYER_DATAMODE_PULL);
70 client_->SetReplaceRule(
true);
71 }
catch (PlayerError &pe) {
73 throw Exception(
"Failed to connect to Player. Error was '%s'", pe.GetErrorStr().c_str());
76 client_->RequestDeviceList();
90 open_fawkes_interfaces();
91 open_player_proxies();
100 PlayerClientThread::open_fawkes_interfaces()
102 std::string prefix =
"/player/interfaces/fawkes/";
105 if (strcmp(vi->
type(),
"string") != 0) {
107 "but found value of type %s",
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",
125 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",
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);
169 unsigned int ifindex = ifindexl;
171 "Adding Player interface %s:%u with name %s",
176 ClientProxy *proxy = NULL;
177 for (std::list<playerc_device_info_t>::iterator i = devices.begin();
178 (proxy == NULL) && (i != devices.end());
180 if ((i->addr.host == host) && (i->addr.robot == robot) && (i->addr.index == ifindex)
181 && (iftype == client_->LookupName(i->addr.interf))) {
184 "Opening Player interface of type %u (%s), "
185 "index %u, host %u, robot %u, driver %s",
187 client_->LookupName(i->addr.interf).c_str(),
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);
205 pmap_[varname] = proxy;
208 "No matching interface found for %s=%s:%u, ignoring",
218 PlayerClientThread::create_mappers()
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",
226 pmap_[i->first]->GetInterfaceStr().c_str());
229 throw Exception(
"No matching proxy found for interface %s (%s)",
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());
245 for (MapperList::iterator m = mappers_.begin(); m != mappers_.end(); ++m) {
250 close_fawkes_interfaces();
251 close_player_proxies();
257 PlayerClientThread::close_fawkes_interfaces()
259 for (InterfaceMap::iterator i = imap_.begin(); i != imap_.end(); ++i) {
266 PlayerClientThread::close_player_proxies()
268 for (ProxyMap::iterator p = pmap_.begin(); p != pmap_.end(); ++p) {
285 for (MapperList::iterator m = mappers_.begin(); m != mappers_.end(); ++m) {
286 (*m)->sync_fawkes_to_player();
288 }
catch (PlayerCc::PlayerError &e) {
289 logger->
log_warn(
name(),
"Failed to update player proxies: %s", e.GetErrorStr().c_str());
297 if (client_->Peek()) {
301 for (MapperList::iterator m = mappers_.begin(); m != mappers_.end(); ++m) {
302 (*m)->sync_player_to_fawkes();
307 }
catch (PlayerCc::PlayerError &e) {
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.
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.
Iterator interface to iterate over config values.
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.
void append(const char *format,...) noexcept
Append messages to the message list.
Base class for all Fawkes BlackBoard interfaces.
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.
NetworkNameResolver * nnresolver
Network name resolver to lookup IP addresses of hostnames and vice versa.
bool resolve_name_blocking(const char *name, struct sockaddr **addr, socklen_t *addrlen)
Resolve name and wait for the result.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.