// Get the particle set
int playerc_localize_get_particles(playerc_localize_t *device)
{
int len;
int i;
player_localize_get_particles_t* req;
assert(req = calloc(1,sizeof(player_localize_get_particles_t)));
req->subtype = PLAYER_LOCALIZE_GET_PARTICLES_REQ;
len = playerc_client_request(device->info.client, &device->info,
req, sizeof(req), req,
sizeof(player_localize_get_particles_t));
if (len < 0)
{
free(req);
return -1;
}
// TODO: better length checking here
// byteswap
device->mean[0] = ((int32_t)ntohl(req->mean[0])) / 1e3;
device->mean[1] = ((int32_t)ntohl(req->mean[1])) / 1e3;
device->mean[2] = (((int32_t)ntohl(req->mean[2])) / 3600.0) * M_PI/180.0;
device->variance = ((uint64_t)ntohll(req->variance)) / (1e3 * 1e3);
device->num_particles = (int32_t)ntohl(req->num_particles);
for(i=0;i<device->num_particles;i++)
{
if(i >= PLAYER_LOCALIZE_PARTICLES_MAX)
{
device->num_particles = i;
PLAYERC_WARN("too many particles");
break;
}
device->particles[i].pose[0] =
((int32_t)ntohl(req->particles[i].pose[0])) / 1e3;
device->particles[i].pose[1] =
((int32_t)ntohl(req->particles[i].pose[1])) / 1e3;
device->particles[i].pose[2] =
(((int32_t)ntohl(req->particles[i].pose[2])) / 3600.0) *
M_PI / 180.0;
device->particles[i].weight =
((uint32_t)ntohl(req->particles[i].alpha)) / 1e6;
}
free(req);
return 0;
}
// Test to see if there is pending data. Don't send a data request.
int playerc_client_internal_peek(playerc_client_t *client, int timeout)
{
int count;
struct pollfd fd;
if (client->sock < 0)
{
PLAYERC_WARN("no socket to peek at");
return -1;
}
fd.fd = client->sock;
//fd.events = POLLIN | POLLHUP;
fd.events = POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL;
fd.revents = 0;
// Wait for incoming data
count = poll(&fd, 1, timeout);
if (count < 0)
{
if(errno == EINTR)
return(0);
else
{
PLAYERC_ERR1("poll returned error [%s]", strerror(errno));
//playerc_client_disconnect(client);
return(playerc_client_disconnect_retry(client));
}
}
if (count > 0 && (fd.revents & POLLHUP))
{
PLAYERC_ERR("socket disconnected");
//playerc_client_disconnect(client);
return(playerc_client_disconnect_retry(client));
}
return count;
}
// Read and process a packet (nonblocking), fills in pointer to proxy that got data
// returns 0 if no data recieved, 1 if data recieved and -1 on error
int playerc_client_read_nonblock_withproxy(playerc_client_t *client, void ** proxy)
{
player_msghdr_t header;
int ret;
while (true)
{
// See if there is any queued data.
if (playerc_client_pop (client, &header, client->data) < 0)
{
// If there is no queued data, peek at the socket
if((ret = playerc_client_internal_peek(client,0)) <= 0)
{
// If we haven't requested sata there will be no SYNCH message to wait further for, thus if we have got data from the internal queue this time
// We need to return true
if (!client->data_requested && client->data_received)
{
client->data_received = 0;
if (proxy)
*proxy = client->id;
return 1;
}
else
return 0;
}
// There's data on the socket, so read a packet (blocking).
if((ret = playerc_client_readpacket (client, &header, client->data)) < 0)
return ret;
}
// One way or another, we got a new packet into (header,client->data),
// so process it
switch(header.type)
{
case PLAYER_MSGTYPE_RESP_ACK:
PLAYERC_WARN ("Discarding unclaimed ACK");
playerxdr_cleanup_message(client->data, header.addr.interf, header.type, header.subtype);
break;
case PLAYER_MSGTYPE_SYNCH:
client->data_requested = 0;
if (header.subtype == PLAYER_PLAYER_SYNCH_OVERFLOW)
{
client->overflow_count += *((uint32_t*)client->data);
}
if(!client->data_received)
{
PLAYERC_WARN ("No data recieved with SYNC");
ret = -1;
}
else
{
if (proxy)
*proxy = client->id;
ret = 1;
}
playerxdr_cleanup_message(client->data, header.addr.interf, header.type, header.subtype);
return ret;
case PLAYER_MSGTYPE_DATA:
client->lasttime = client->datatime;
client->datatime = header.timestamp;
if (client->mode == PLAYER_DATAMODE_PUSH)
{
// If in push mode, handle and return
void *result = playerc_client_dispatch (client, &header, client->data);
// Need to ensure that any dynamic data made during unpacking is cleaned up
playerxdr_cleanup_message(client->data, header.addr.interf, header.type, header.subtype);
if (proxy)
*proxy = result;
return 1;
}
else // PULL mode, so keep on going
{
void *result = playerc_client_dispatch (client, &header, client->data);
playerxdr_cleanup_message(client->data, header.addr.interf, header.type, header.subtype);
client->data_received = 1;
if (result == NULL)
{
PLAYERC_WARN1 ("Failed to dispatch data message: subtype %d", header.subtype);
printf("address: %u:%u:%s:%u\nsize: %u",
header.addr.host,
header.addr.robot,
interf_to_str(header.addr.interf),
header.addr.index,
header.size);
return -1;
}
break;
}
default:
playerxdr_cleanup_message(client->data, header.addr.interf, header.type, header.subtype);
PLAYERC_WARN1 ("unexpected message type [%s]", msgtype_to_str(header.type));
PLAYERC_WARN5 ("address: %u:%u:%s:%u\nsize: %u",
header.addr.host,
header.addr.robot,
interf_to_str(header.addr.interf),
header.addr.index,
header.size);
return -1;
}
}
}
// Disconnect from the server, with potential retry
int playerc_client_disconnect_retry(playerc_client_t *client)
{
int retval;
int i;
int j;
struct timespec sleeptime;
sleeptime.tv_sec = (long) client->retry_time;
sleeptime.tv_nsec = 0;
/* Disconnect */
if((retval = playerc_client_disconnect(client)) != 0)
PLAYERC_WARN("playerc_client_disconnect() failed");
for(j=0; (client->retry_limit < 0) || (j<client->retry_limit); j++)
{
PLAYERC_WARN1("Reconnecting, attempt %d", j);
/* Reconnect */
if((retval = playerc_client_connect(client)) != 0)
PLAYERC_WARN("playerc_client_connect() failed");
else
{
/* Clean out buffers */
client->read_xdrdata_len = 0;
/* TODO: re-establish replacement rules, delivery modes, etc. */
/* Re-subscribe to devices */
for(i=0;i<client->device_count;i++)
{
if(client->device[i]->subscribed)
{
// TODO: what should access be here?
if((retval = playerc_device_subscribe(client->device[i],
PLAYERC_OPEN_MODE)) != 0)
{
PLAYERC_WARN2("playerc_device_subscribe() failed for %d:%d",
client->device[i]->addr.interf,
client->device[i]->addr.index);
// TODO: Subscription failed for one device; should we give up?
if(playerc_client_disconnect(client) != 0)
PLAYERC_WARN("playerc_client_disconnect() failed");
break;
}
}
}
// Did we get all of them?
if(i == client->device_count)
break;
}
nanosleep(&sleeptime,NULL);
}
if((client->retry_limit < 0) || (j < client->retry_limit))
{
PLAYERC_WARN("successfully reconnected");
return(0);
}
else
{
PLAYERC_WARN("failed to reconnect");
return(-1);
}
}
// Connect to the server
int playerc_client_connect(playerc_client_t *client)
{
#if defined(HAVE_GETADDRINFO)
struct addrinfo* addr_ptr = NULL;
#else
struct hostent* entp = NULL;
#endif
char banner[PLAYER_IDENT_STRLEN];
int ret;
//double t;
/*
struct timeval last;
struct timeval curr;
*/
#if defined (WIN32)
unsigned long setting = 0;
#else
int old_flags;
struct itimerval timer;
struct sigaction sigact;
#endif
struct sockaddr_in clientaddr;
// Construct socket
if(client->transport == PLAYERC_TRANSPORT_UDP)
{
#if defined (WIN32)
if((client->sock = socket(PF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET)
#else
if((client->sock = socket(PF_INET, SOCK_DGRAM, 0)) < 0)
#endif
{
STRERROR(PLAYERC_ERR2, "socket() failed with error [%d: %s]");
return -1;
}
/*
* INADDR_ANY indicates that any network interface (IP address)
* for the local host may be used (presumably the OS will choose the
* right one).
*
* Specifying sin_port = 0 allows the system to choose the port.
*/
clientaddr.sin_family = AF_INET;
clientaddr.sin_addr.s_addr = INADDR_ANY;
clientaddr.sin_port = 0;
if(bind(client->sock,
(struct sockaddr*)&clientaddr, sizeof(clientaddr)) < -1)
{
STRERROR(PLAYERC_ERR2, "bind() failed with error [%d: %s]");
return -1;
}
}
else
{
#if defined (WIN32)
if((client->sock = socket(PF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET)
#else
if((client->sock = socket(PF_INET, SOCK_STREAM, 0)) < 0)
#endif
{
STRERROR(PLAYERC_ERR2, "socket() failed with error [%d: %s]");
return -1;
}
}
#if ENABLE_TCP_NODELAY
// Disable Nagel's algorithm for lower latency
{
int yes = 1;
if(setsockopt(client->sock, IPPROTO_TCP, TCP_NODELAY, &yes,
sizeof(int)) == -1)
{
PLAYERC_ERR("failed to enable TCP_NODELAY - setsockopt failed");
return -1;
}
}
#endif
// Construct server address
memset(&client->server, 0, sizeof(client->server));
client->server.sin_family = AF_INET;
client->server.sin_port = htons(client->port);
#if defined(HAVE_GETADDRINFO)
if (getaddrinfo(client->host, NULL, NULL, &addr_ptr) != 0)
{
playerc_client_disconnect(client);
PLAYERC_ERR("getaddrinfo() failed with error");
return -1;
}
assert(addr_ptr);
assert(addr_ptr->ai_addr);
#ifdef AF_INET6
if (((addr_ptr->ai_addr->sa_family) != AF_INET) &&
((addr_ptr->ai_addr->sa_family) != AF_INET6))
#else
if ((addr_ptr->ai_addr->sa_family) != AF_INET)
#endif
{
playerc_client_disconnect(client);
PLAYERC_ERR("unsupported internet address family");
return -1;
}
client->server.sin_addr.s_addr = ((struct sockaddr_in *)(addr_ptr->ai_addr))->sin_addr.s_addr;
freeaddrinfo(addr_ptr);
addr_ptr = NULL;
#else
entp = gethostbyname(client->host);
if (entp == NULL)
{
playerc_client_disconnect(client);
STRERROR(PLAYERC_ERR2, "gethostbyname() failed with error [%d: %s]");
return -1;
}
assert(entp->h_length <= sizeof (client->server.sin_addr));
memcpy(&(client->server.sin_addr), entp->h_addr_list[0], entp->h_length);
#endif
// Connect the socket
/*
t = client->request_timeout;
do
{
if (t <= 0)
{
PLAYERC_ERR2("connect call on [%s:%d] timed out",
client->host, client->port);
return -1;
}
gettimeofday(&last,NULL);
puts("calling connect");
ret = connect(client->sock, (struct sockaddr*)&client->server,
sizeof(client->server));
gettimeofday(&curr,NULL);
t -= ((curr.tv_sec + curr.tv_usec/1e6) -
(last.tv_sec + last.tv_usec/1e6));
} while (ret == -1 && (errno == EALREADY || errno == EAGAIN || errno == EINPROGRESS));
*/
// In Windows, the connect timeout is (apparently) a registry setting.
#if !defined (WIN32)
/* Set up a timer to interrupt the connection process */
timer.it_interval.tv_sec = 0;
timer.it_interval.tv_usec = 0;
timer.it_value.tv_sec = (int)floor(client->request_timeout);
timer.it_value.tv_usec =
(int)rint(fmod(client->request_timeout,timer.it_value.tv_sec)*1e6);
if(setitimer(ITIMER_REAL, &timer, NULL) != 0)
PLAYERC_WARN("failed to set up connection timer; "
"indefinite hang may result");
/* Turn off system call restart so that connect() will terminate when the
* alarm goes off */
if(sigaction(SIGALRM, NULL, &sigact) != 0)
PLAYERC_WARN("failed to get SIGALRM action data; "
"unexpected exit may result");
else
{
#ifdef SA_RESTART
sigact.sa_handler = dummy;
sigact.sa_flags &= ~SA_RESTART;
if(sigaction(SIGALRM, &sigact, NULL) != 0)
#endif
PLAYERC_WARN("failed to set SIGALRM action data; "
"unexpected exit may result");
}
#endif
ret = connect(client->sock, (struct sockaddr*)&client->server,
sizeof(client->server));
#if !defined (WIN32)
/* Turn off timer */
timer.it_value.tv_sec = 0;
timer.it_value.tv_usec = 0;
if(setitimer(ITIMER_REAL, &timer, NULL) != 0)
PLAYERC_WARN("failed to turn off connection timer; "
"unexpected exit may result");
/* Restore normal SIGALRM behavior */
#ifdef SA_RESTART
sigact.sa_handler = SIG_DFL;
sigact.sa_flags |= SA_RESTART;
if(sigaction(SIGALRM, &sigact, NULL) != 0)
#endif
PLAYERC_WARN("failed to reset SIGALRM action data; "
"unexpected behavior may result");
#endif
if (ret < 0)
{
playerc_client_disconnect(client);
#if defined (WIN32)
FormatMessage (FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
ErrNo, 0, (LPTSTR) &errBuffer, 0, NULL);
PLAYERC_ERR4 ("connect call on [%s:%d] failed with error [%d:%s]",
client->host, client->port, ErrNo, (LPTSTR) errBuffer);
LocalFree (errBuffer);
#else
PLAYERC_ERR4("connect call on [%s:%d] failed with error [%d:%s]",
client->host, client->port, errno, strerror(ErrNo));
#endif
return -1;
}
// For UDP, send an empty msg to get things going
if(client->transport == PLAYERC_TRANSPORT_UDP)
{
if(send(client->sock, NULL, 0, 0) < 0)
{
STRERROR(PLAYERC_ERR2, "send() failed with error [%d: %s]");
return -1;
}
}
// set socket to be blocking
#if defined (WIN32)
if (ioctlsocket (client->sock, FIONBIO, &setting) == SOCKET_ERROR)
{
FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
ErrNo, 0, (LPTSTR) &errBuffer, 0, NULL);
PLAYERC_ERR1("error getting socket flags [%s]", (LPTSTR) errBuffer);
LocalFree(errBuffer);
}
#else
if ((old_flags = fcntl(client->sock, F_GETFL)) < 0)
{
PLAYERC_ERR1("error getting socket flags [%s]", strerror(errno));
return -1;
}
if (fcntl(client->sock, F_SETFL, old_flags & ~O_NONBLOCK) < 0)
{
PLAYERC_ERR1("error setting socket non-blocking [%s]", strerror(errno));
return -1;
}
#endif
// Get the banner
if (timed_recv(client->sock, banner, sizeof(banner), 0, 2000) < sizeof(banner))
{
playerc_client_disconnect(client);
PLAYERC_ERR("incomplete initialization string");
return -1;
}
//set the datamode to pull
playerc_client_datamode(client, PLAYER_DATAMODE_PULL);
PLAYERC_WARN4("[%s] connected on [%s:%d] with sock %d\n", banner, client->host, client->port, client->sock);
client->connected = 1;
return 0;
}
// Write a raw packet
int playerc_client_writepacket(playerc_client_t *client,
player_msghdr_t *header, const char *data)
{
int bytes, ret, length;
player_pack_fn_t packfunc;
int encode_msglen;
struct timeval curr;
if (client->sock < 0)
{
PLAYERC_WARN("no socket to write to");
return -1;
}
// Encode the body first, if it's non-NULL
if(data)
{
// Locate the appropriate packing function for the message body
if(!(packfunc = playerxdr_get_packfunc(header->addr.interf,
header->type,
header->subtype)))
{
// TODO: Allow the user to register a callback to handle unsupported
// messages
PLAYERC_ERR4("skipping message to %s:%u with unsupported type %s:%u",
interf_to_str(header->addr.interf), header->addr.index, msgtype_to_str(header->type), header->subtype);
return(-1);
}
if((encode_msglen =
(*packfunc)(client->write_xdrdata + PLAYERXDR_MSGHDR_SIZE,
PLAYER_MAX_MESSAGE_SIZE - PLAYERXDR_MSGHDR_SIZE,
(void*) data, PLAYERXDR_ENCODE)) < 0)
{
PLAYERC_ERR4("encoding failed on message from %s:%u with type %s:%u",
interf_to_str(header->addr.interf), header->addr.index, msgtype_to_str(header->type), header->subtype);
return(-1);
}
}
else
encode_msglen = 0;
// Write in the encoded size and current time
header->size = encode_msglen;
gettimeofday(&curr,NULL);
header->timestamp = curr.tv_sec + curr.tv_usec / 1e6;
// Pack the header
if(player_msghdr_pack(client->write_xdrdata, PLAYERXDR_MSGHDR_SIZE,
header, PLAYERXDR_ENCODE) < 0)
{
PLAYERC_ERR("failed to pack header");
return -1;
}
// Send the message
length = PLAYERXDR_MSGHDR_SIZE + encode_msglen;
bytes = PLAYERXDR_MSGHDR_SIZE + encode_msglen;
do
{
ret = send(client->sock, &client->write_xdrdata[length-bytes],
bytes, 0);
if (ret > 0)
{
bytes -= ret;
}
#if defined (WIN32)
else if (ret < 0 && (errno != ERRNO_EAGAIN && errno != WSAEINPROGRESS))
#else
else if (ret < 0 && (errno != ERRNO_EAGAIN && errno != EINPROGRESS && errno != EWOULDBLOCK))
#endif
{
STRERROR (PLAYERC_ERR2, "send on body failed with error [%d: %s]");
//playerc_client_disconnect(client);
return(playerc_client_disconnect_retry(client));
}
} while (bytes);
return 0;
}
// Read a raw packet
int playerc_client_readpacket(playerc_client_t *client,
player_msghdr_t *header,
char *data)
{
int nbytes;
player_pack_fn_t packfunc;
int decode_msglen;
if (client->sock < 0)
{
PLAYERC_WARN("no socket to read from");
return -1;
}
while(client->read_xdrdata_len < PLAYERXDR_MSGHDR_SIZE)
{
nbytes = timed_recv(client->sock,
client->read_xdrdata + client->read_xdrdata_len,
PLAYERXDR_MSGHDR_SIZE - client->read_xdrdata_len,
0, (int) client->request_timeout * 1000);
if (nbytes <= 0)
{
if(nbytes == 0)
return -1;
if(errno == EINTR)
continue;
else
{
STRERROR (PLAYERC_ERR2, "recv failed with error [%d: %s]");
//playerc_client_disconnect(client);
if(playerc_client_disconnect_retry(client) < 0)
return(-1);
else
continue;
}
}
client->read_xdrdata_len += nbytes;
}
// Unpack the header
if(player_msghdr_pack(client->read_xdrdata,
PLAYERXDR_MSGHDR_SIZE,
header, PLAYERXDR_DECODE) < 0)
{
PLAYERC_ERR("failed to unpack header");
return -1;
}
if (header->size > PLAYERXDR_MAX_MESSAGE_SIZE - PLAYERXDR_MSGHDR_SIZE)
{
PLAYERC_WARN1("packet is too large, %d bytes", header->size);
}
// Slide over the header
memmove(client->read_xdrdata,
client->read_xdrdata + PLAYERXDR_MSGHDR_SIZE,
client->read_xdrdata_len - PLAYERXDR_MSGHDR_SIZE);
client->read_xdrdata_len -= PLAYERXDR_MSGHDR_SIZE;
while(client->read_xdrdata_len < header->size)
{
nbytes = timed_recv(client->sock,
client->read_xdrdata + client->read_xdrdata_len,
header->size - client->read_xdrdata_len,
0, (int) client->request_timeout*1000);
if (nbytes <= 0)
{
if(errno == EINTR)
continue;
{
STRERROR (PLAYERC_ERR2, "recv failed with error [%d: %s]");
//playerc_client_disconnect(client);
if(playerc_client_disconnect_retry(client) < 0)
return(-1);
else
{
/* Need to start over; the easiest way is to recursively call
* myself. Might be problematic... */
return(playerc_client_readpacket(client,header,data));
}
}
}
client->read_xdrdata_len += nbytes;
}
if (header->size)
{
// Locate the appropriate unpacking function for the message body
if(!(packfunc = playerxdr_get_packfunc(header->addr.interf, header->type,
header->subtype)))
{
// TODO: Allow the user to register a callback to handle unsupported
// messages
PLAYERC_ERR4("skipping message from %s:%u with unsupported type %s:%u",
interf_to_str(header->addr.interf), header->addr.index, msgtype_to_str(header->type), header->subtype);
// Slide over the body
memmove(client->read_xdrdata,
client->read_xdrdata + header->size,
client->read_xdrdata_len - header->size);
client->read_xdrdata_len -= header->size;
return(-1);
}
// Unpack the body
if((decode_msglen = (*packfunc)(client->read_xdrdata,
header->size, data, PLAYERXDR_DECODE)) < 0)
{
PLAYERC_ERR4("decoding failed on message from %s:%u with type %s:%u",
interf_to_str(header->addr.interf), header->addr.index, msgtype_to_str(header->type), header->subtype);
return(-1);
}
}
else
{
decode_msglen = 0;
}
// Slide over the body
memmove(client->read_xdrdata,
client->read_xdrdata + header->size,
client->read_xdrdata_len - header->size);
client->read_xdrdata_len -= header->size;
// Rewrite the header with the decoded message length
header->size = decode_msglen;
return 0;
}
