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; }