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) {