void
browse_cb(player_sd_t* sd, player_sd_dev_t* dev)
{
if(dev->interf == PLAYER_LASER_CODE)
{
clients[num_laserdevs] = playerc_client_create(mclient,
dev->hostname,
dev->robot);
if(0 != playerc_client_connect(clients[num_laserdevs]))
exit(-1);
playerc_client_datamode(clients[num_laserdevs], PLAYERC_DATAMODE_PUSH);
// Create and subscribe to a laser device.
lasers[num_laserdevs] = playerc_laser_create(clients[num_laserdevs],
dev->index);
if(playerc_laser_subscribe(lasers[num_laserdevs], PLAYER_OPEN_MODE))
exit(-1);
// Add a callback to be invoked whenever we receive new data from this
// laser
playerc_client_addcallback(clients[num_laserdevs],
&(lasers[num_laserdevs]->info),
device_cb, lasers[num_laserdevs]);
num_laserdevs++;
printf("subscribed to: %s:%u:%s:%u\n",
dev->hostname,
dev->robot,
interf_to_str(dev->interf),
dev->index);
printf("Now receiving %d lasers\n", num_laserdevs);
}
}
int
player_sd_register(player_sd_t* sd,
const char* name,
player_devaddr_t addr)
{
DNSServiceErrorType sdErr;
char recordval[PLAYER_SD_TXT_MAXLEN];
int i,j;
player_sd_mdns_t* mdns = (player_sd_mdns_t*)(sd->sdRef);
player_sd_mdns_dev_t* dev;
char nameBuf[PLAYER_SD_NAME_MAXLEN];
// Find a spot for this device
for(i=0;i<mdns->mdnsDevs_len;i++)
{
if(!mdns->mdnsDevs[i].valid)
break;
}
if(i==mdns->mdnsDevs_len)
{
// Make the list bigger
if(!mdns->mdnsDevs_len)
mdns->mdnsDevs_len = PLAYER_SD_MDNS_DEVS_LEN_INITIAL;
else
mdns->mdnsDevs_len *= PLAYER_SD_MDNS_DEVS_LEN_MULTIPLIER;
mdns->mdnsDevs =
(player_sd_mdns_dev_t*)realloc(mdns->mdnsDevs,
(mdns->mdnsDevs_len *
sizeof(player_sd_mdns_dev_t)));
assert(mdns->mdnsDevs);
for(j=i;j<mdns->mdnsDevs_len;j++)
mdns->mdnsDevs[j].valid = 0;
}
dev = mdns->mdnsDevs + i;
dev->fail = 0;
memset(dev->sdDev.name,0,sizeof(dev->sdDev.name));
strncpy(dev->sdDev.name,name,sizeof(dev->sdDev.name)-1);
memset(dev->sdDev.hostname,0,sizeof(dev->sdDev.hostname));
packedaddr_to_dottedip(dev->sdDev.hostname,sizeof(dev->sdDev.hostname),
addr.host);
dev->sdDev.robot = addr.robot;
dev->sdDev.interf = addr.interf;
dev->sdDev.index = addr.index;
dev->nameIdx = 1;
TXTRecordCreate(&(dev->txtRecord),sizeof(dev->txtBuf),dev->txtBuf);
memset(recordval,0,sizeof(recordval));
snprintf(recordval, sizeof(recordval), "%s:%u",
interf_to_str(addr.interf), addr.index);
if((sdErr = TXTRecordSetValue(&(dev->txtRecord),
"device",
strlen(recordval),
recordval)))
{
PLAYER_ERROR1("TXTRecordSetValue returned error: %d", sdErr);
return(-1);
}
memset(nameBuf,0,sizeof(nameBuf));
strncpy(nameBuf,name,sizeof(nameBuf)-1);
sdErr = kDNSServiceErr_NameConflict;
// Avahi can return the kDNSServiceErr_NameConflict immediately.
while(sdErr == kDNSServiceErr_NameConflict)
{
sdErr = DNSServiceRegister(&(dev->regRef),
0,
0,
nameBuf,
PLAYER_SD_SERVICENAME,
NULL,
NULL,
addr.robot,
TXTRecordGetLength(&(dev->txtRecord)),
TXTRecordGetBytesPtr(&(dev->txtRecord)),
registerCB,
(void*)dev);
if(sdErr == kDNSServiceErr_NameConflict)
{
// Pick a new name
memset(nameBuf,0,sizeof(nameBuf));
snprintf(nameBuf,sizeof(nameBuf),"%s (%d)",
name,dev->nameIdx++);
}
}
if(sdErr != kDNSServiceErr_NoError)
{
PLAYER_ERROR1("DNSServiceRegister returned error: %d", sdErr);
return(-1);
}
else
{
dev->valid = 1;
if(strcmp(nameBuf,name))
PLAYER_WARN2("Changing service name of %s to %s\n",
name,nameBuf);
PLAYER_MSG1(2,"Registration of %s successful", name);
return(0);
}
}
// Issue request and await reply (blocking).
int playerc_client_request(playerc_client_t *client,
playerc_device_t *deviceinfo,
uint8_t subtype,
const void *req_data, void **rep_data)
{
int peek;
struct timeval start;
struct timeval curr;
player_msghdr_t req_header, rep_header;
memset(&req_header, 0, sizeof(req_header));
if(deviceinfo == NULL)
{
req_header.addr.interf = PLAYER_PLAYER_CODE;
req_header.type = PLAYER_MSGTYPE_REQ;
}
else
{
req_header.addr = deviceinfo->addr;
req_header.type = PLAYER_MSGTYPE_REQ;
}
req_header.subtype = subtype;
if (playerc_client_writepacket(client, &req_header, req_data) < 0)
return -1;
// Read packets until we get a reply. Data packets get queued up
// for later processing.
gettimeofday(&start,NULL);
for(curr = start; tdiff(start,curr)< client->request_timeout; gettimeofday(&curr,NULL))
{
// Peek at the socket
if((peek = playerc_client_internal_peek(client,10)) < 0)
return -1;
else if(peek == 0)
continue;
// There's data on the socket, so read a packet (blocking).
if(playerc_client_readpacket(client, &rep_header, client->data) < 0)
return -1;
if (rep_header.type == PLAYER_MSGTYPE_DATA || rep_header.type == PLAYER_MSGTYPE_SYNCH)
{
// Queue up any incoming data and sync packets for later processing
playerc_client_push(client, &rep_header, client->data);
}
else if(rep_header.type == PLAYER_MSGTYPE_RESP_ACK)
{
// Using TCP, we only need to check the interface and index
if (rep_header.addr.interf != req_header.addr.interf ||
rep_header.addr.index != req_header.addr.index ||
rep_header.subtype != req_header.subtype)
{
PLAYERC_ERR6("got the wrong kind of reply (%d %d %d != %d %d %d).",rep_header.addr.interf,
rep_header.addr.index, rep_header.subtype, req_header.addr.interf, req_header.addr.index,
req_header.subtype);
return -1;
}
if (rep_header.size > 0)
{
if (rep_data)
{
*rep_data = playerxdr_clone_message(client->data,rep_header.addr.interf, rep_header.type, rep_header.subtype);
}
playerxdr_cleanup_message(client->data,rep_header.addr.interf, rep_header.type, rep_header.subtype);
}
return(0);
}
else if (rep_header.type == PLAYER_MSGTYPE_RESP_NACK)
{
// Using TCP, we only need to check the interface and index
if (rep_header.addr.interf != req_header.addr.interf ||
rep_header.addr.index != req_header.addr.index ||
rep_header.subtype != req_header.subtype)
{
PLAYERC_ERR("got the wrong kind of reply (not good).");
return -1;
}
PLAYERC_ERR("got NACK from request");
return -2;
}
}
PLAYERC_ERR4("timed out waiting for server reply to request %s:%d:%s:%d", interf_to_str(req_header.addr.interf), req_header.addr.index, msgtype_to_str(req_header.type), req_header.subtype);
return -1;
}
// 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;
}
}
}
// 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;
}
// Main
int main(int argc, char **argv)
{
playerc_client_t *client;
rtk_app_t *app;
mainwnd_t *mainwnd;
opt_t *opt;
const char *host;
int port;
int i;
int count;
double rate;
char section[256];
int device_count;
device_t devices[PLAYER_MAX_DEVICES];
device_t *device;
struct timeval tv, tc = {0, 0};
struct timespec st = {0, (1.0/GUI_UPDATE_RATE) * 1e9};
printf("PlayerViewer %s\n", PLAYER_VERSION);
// Initialise rtk lib (after we have read the program options we
// want).
rtk_init(&argc, &argv);
// Register signal handlers
signal(SIGINT, sig_quit);
signal(SIGQUIT, sig_quit);
// Load program options
opt = opt_init(argc, argv, NULL);
if (!opt)
{
print_usage();
return -1;
}
// Pick out some important program options
host = opt_get_string(opt, "", "host", NULL);
if (!host)
host = opt_get_string(opt, "", "h", "localhost");
port = opt_get_int(opt, "", "port", -1);
if (port < 0)
port = opt_get_int(opt, "", "p", 6665);
rate = opt_get_double(opt, "", "rate", 5.0);
if(rate < 0.0)
rate = 0.0;
// Connect to the server
printf("Connecting to [%s:%d]\n", host, port);
client = playerc_client_create(NULL, host, port);
if (playerc_client_connect(client) != 0)
{
PRINT_ERR1("%s", playerc_error_str());
print_usage();
return -1;
}
if(rate == 0.0)
{
printf("Setting delivery mode to PLAYER_DATAMODE_PUSH\n");
// Change the server's data delivery mode.
if (playerc_client_set_replace_rule(client, -1, -1, -1, -1, 0) != 0)
{
PRINT_ERR1("%s", playerc_error_str());
return -1;
}
// Change the server's data delivery mode.
// PLAYERC_DATAMODE_PUSH, PLAYERC_DATAMODE_PULL
if (playerc_client_datamode(client, PLAYERC_DATAMODE_PUSH) != 0)
{
PRINT_ERR1("%s", playerc_error_str());
return -1;
}
}
// Get the available devices.
if (playerc_client_get_devlist(client) != 0)
{
PRINT_ERR1("%s", playerc_error_str());
return -1;
}
// Create gui
app = rtk_app_create();
// Create a window for most of the sensor data
mainwnd = mainwnd_create(app, host, port);
if (!mainwnd)
return -1;
// Create a list of available devices, with their gui proxies.
device_count = 0;
for (i = 0; i < client->devinfo_count; i++)
{
device = devices + device_count;
device->addr = client->devinfos[i].addr;
device->drivername = strdup(client->devinfos[i].drivername);
// See if the device should be subscribed immediately.
snprintf(section, sizeof(section), "%s:%d",
interf_to_str(device->addr.interf), device->addr.index);
device->subscribe = opt_get_int(opt, section, "", 0);
device->subscribe = opt_get_int(opt, section, "subscribe", device->subscribe);
if (device->addr.index == 0)
{
snprintf(section, sizeof(section), "%s",
interf_to_str(device->addr.interf));
device->subscribe = opt_get_int(opt, section, "", device->subscribe);
device->subscribe = opt_get_int(opt, section, "subscribe", device->subscribe);
}
// Allow for --position instead of --position2d
if(device->addr.interf == PLAYER_POSITION2D_CODE)
{
snprintf(section, sizeof(section), "%s:%d",
PLAYER_POSITION2D_STRING, device->addr.index);
device->subscribe = opt_get_int(opt, section, "", device->subscribe);
device->subscribe = opt_get_int(opt, section, "subscribe", device->subscribe);
if (device->addr.index == 0)
{
snprintf(section, sizeof(section), "%s", PLAYER_POSITION2D_STRING);
device->subscribe = opt_get_int(opt, section, "", device->subscribe);
device->subscribe = opt_get_int(opt, section, "subscribe", device->subscribe);
}
}
// Create the GUI proxy for this device.
create_proxy(device, opt, mainwnd, client);
device_count++;
}
// Print the list of available devices.
printf("Available devices: %s:%d\n", host, port);
for (i = 0; i < device_count; i++)
{
device = devices + i;
snprintf(section, sizeof(section), "%s:%d",
interf_to_str(device->addr.interf), device->addr.index);
printf("%-16s %-40s", section, device->drivername);
if (device->proxy)
{
if (device->subscribe)
printf("subscribed");
else
printf("ready");
}
else
printf("unsupported");
printf("\n");
}
// Print out a list of unused options.
opt_warn_unused(opt);
// Start the gui; dont run in a separate thread and dont let it do
// its own updates.
rtk_app_main_init(app);
// start out timer if in pull mode
if(rate > 0.0)
gettimeofday(&tv, NULL);
while (!quit)
{
// Let gui process messages
rtk_app_main_loop(app);
if(rate == 0.0) // if we're in push mode
{
// see if there's data
count = playerc_client_peek(client, 50);
if (count < 0)
{
PRINT_ERR1("%s", playerc_error_str());
break;
}
if (count > 0)
{
/*proxy = */playerc_client_read_nonblock(client);
}
}
else // we're in pull mode
{
// we only want to request new data at the target rate
gettimeofday(&tc, NULL);
if(((tc.tv_sec - tv.tv_sec) + (tc.tv_usec - tv.tv_usec)/1e6) > 1.0/rate)
{
tv = tc;
// this requests a round of data from the server to be read
playerc_client_requestdata(client);
playerc_client_read_nonblock(client);
}
else
{
// sleep for the minimum time we can, so we don't use up too much
// processor
nanosleep(&st, NULL);
}
}
// Update the devices
for (i = 0; i < device_count; i++)
{
device = devices + i;
if(device->proxy)
(*(device->fnupdate)) (device->proxy);
}
// Update the main window
if (mainwnd_update(mainwnd) != 0)
break;
}
// Stop the gui
rtk_app_main_term(app);
// Destroy devices
for (i = 0; i < device_count; i++)
{
device = devices + i;
if (device->proxy)
(*(device->fndestroy)) (device->proxy);
free(device->drivername);
}
// Disconnect from server
if (playerc_client_disconnect(client) != 0)
{
PRINT_ERR1("%s", playerc_error_str());
return -1;
}
playerc_client_destroy(client);
// For some reason, either of the following calls makes the program
// segfault on exit. I haven't figured out why, so I'm commenting them out. - BPG
// Destroy the windows
//mainwnd_destroy(mainwnd);
// Destroy the gui
//rtk_app_destroy(app);
opt_term(opt);
return 0;
}
