// Process incoming data void playerc_rfid_putmsg (playerc_rfid_t *device, player_msghdr_t *header, void *data) { int i; if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RFID_DATA_TAGS)) { player_rfid_data_t* rfid_data = (player_rfid_data_t*)data; // clean up our existing tags if (device->tags) { for (i = 0; i < device->tags_count; i++) free(device->tags[i].guid); } device->tags_count = rfid_data->tags_count; device->tags = realloc(device->tags,device->tags_count * sizeof(device->tags[0])); memset(device->tags,0,device->tags_count * sizeof(device->tags[0])); for (i = 0; i < device->tags_count; i++) { device->tags[i].type = rfid_data->tags[i].type; device->tags[i].guid_count = rfid_data->tags[i].guid_count; device->tags[i].guid = malloc(device->tags[i].guid_count); memcpy(device->tags[i].guid,rfid_data->tags[i].guid,device->tags[i].guid_count); } } else PLAYERC_WARN2("skipping rfid message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// Process incoming data void playerc_position3d_putmsg(playerc_position3d_t *device, player_msghdr_t *header, player_position3d_data_t *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_POSITION3D_DATA_STATE)) { device->pos_x = data->pos.px; device->pos_y = data->pos.py; device->pos_z = data->pos.pz; device->pos_roll = data->pos.proll; device->pos_pitch = data->pos.ppitch; device->pos_yaw = data->pos.pyaw; device->vel_x = data->vel.px; device->vel_y = data->vel.py; device->vel_z = data->vel.pz; device->vel_roll = data->vel.proll; device->vel_pitch = data->vel.ppitch; device->vel_yaw = data->vel.pyaw; device->stall = data->stall; } else PLAYERC_WARN2("skipping position3d message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// Process incoming data void playerc_gps_putmsg(playerc_gps_t *device, player_msghdr_t *header, void *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_GPS_DATA_STATE)) { player_gps_data_t * gps_data = (player_gps_data_t *) data; device->utc_time = gps_data->time_sec + ((double) gps_data->time_usec)/1e6; device->lat = gps_data->latitude / 1e7; device->lon = gps_data->longitude / 1e7; device->alt = gps_data->altitude / 1e3; device->utm_e = gps_data->utm_e / 100.0; device->utm_n = gps_data->utm_n / 100.0; device->hdop = gps_data->hdop / 10.0; device->vdop = gps_data->vdop / 10.0; device->err_horz = gps_data->err_horz; device->err_vert = gps_data->err_vert; device->quality = gps_data->quality; device->sat_count = gps_data->num_sats; } else PLAYERC_WARN2("skipping gps message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); return; }
// Process incoming data void playerc_micronsonar_putmsg(playerc_micronsonar_t *device, player_msghdr_t *header, player_micronsonar_data_t *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_MICRONSONAR_DATA_STATE)) { device->width = data->width; device->height = data->height; device->bpp = data->bpp; device->format = data->format; device->image_count = data->image_count; device->image = realloc(device->image, sizeof(device->image[0])*device->image_count); device->centreX = data->centreX; device->centreY = data->centreY; device->range = data->range; device->numBins = data->numBins; device->startAngle = data->startAngle; device->endAngle = data->endAngle; if (device->image) memcpy(device->image, data->image, device->image_count); else PLAYERC_ERR1("failed to allocate memory for image, needed %ld bytes\n", sizeof(device->image[0])*device->image_count); } else PLAYERC_WARN2("skipping micronsonar message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); return; }
// Process incoming data void playerc_ranger_putmsg(playerc_ranger_t *device, player_msghdr_t *header, void *data, size_t len) { if(header->size == 0) { PLAYERC_ERR2("(putmsg) Ranger message size <= 0 in message %s/%d", msgtype_to_str(header->type), header->subtype); return; } if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_RANGE)) { playerc_ranger_copy_range_data(device, (player_ranger_data_range_t *) data); playerc_ranger_calculate_bearings(device); playerc_ranger_calculate_points(device); } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_RANGESTAMPED)) { player_ranger_data_rangestamped_t *stampedData = (player_ranger_data_rangestamped_t*) data; playerc_ranger_copy_range_data(device, &stampedData->data); if (stampedData->have_geom) playerc_ranger_copy_geom(device, &stampedData->geom); if (stampedData->have_config) playerc_ranger_copy_config(device, &stampedData->config); playerc_ranger_calculate_bearings(device); playerc_ranger_calculate_points(device); } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_INTNS)) { playerc_ranger_copy_intns_data(device, (player_ranger_data_intns_t *) data); } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_INTNSSTAMPED)) { player_ranger_data_intnsstamped_t *stampedData = (player_ranger_data_intnsstamped_t*) data; playerc_ranger_copy_intns_data(device, &stampedData->data); if (stampedData->have_geom) playerc_ranger_copy_geom(device, &stampedData->geom); if (stampedData->have_config) playerc_ranger_copy_config(device, &stampedData->config); } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_GEOM)) { playerc_ranger_copy_geom(device, (player_ranger_geom_t *) data); } else PLAYERC_WARN2("Skipping ranger message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// Process incoming data void playerc_time_putmsg(playerc_time_t *device, player_msghdr_t *header, void *data, size_t len) { PLAYERC_WARN2("skipping time message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); return; }
// Process incoming data void playerc_opaque_putmsg(playerc_opaque_t *device, player_msghdr_t *header, player_opaque_data_t *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_OPAQUE_DATA_STATE)) { device->data_count = data->data_count; device->data = realloc(device->data, sizeof(*device->data)*device->data_count); memcpy(device->data, data->data, device->data_count); } else PLAYERC_WARN2("skipping opaque message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); return; }
// Process incoming data void playerc_position1d_putmsg(playerc_position1d_t *device, player_msghdr_t *header, player_position1d_data_t *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_POSITION1D_DATA_STATE)) { device->pos = data->pos; device->vel = data->vel; device->stall = data->stall; device->status = data->status; } else PLAYERC_WARN2("skipping position1d message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// Process incoming data void playerc_pointcloud3d_putmsg (playerc_pointcloud3d_t *device, player_msghdr_t *header, void *data) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_POINTCLOUD3D_DATA_STATE)) { player_pointcloud3d_data_t* pc3_data = (player_pointcloud3d_data_t*)data; device->points_count = pc3_data->points_count; device->points = realloc(device->points, device->points_count*sizeof(device->points[0])); memcpy (device->points, pc3_data->points, sizeof (player_pointcloud3d_element_t)*device->points_count); } else PLAYERC_WARN2("skipping pointcloud3d message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// Process incoming data void playerc_motor_putmsg(playerc_motor_t *device, player_msghdr_t *header, player_motor_data_t *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_MOTOR_DATA_STATE)) { device->pt = data->pos; device->vt = data->vel; device->limits = data->limits; device->stall = data->stall; } else PLAYERC_WARN2("skipping motor message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
void playerc_audio_putmsg(playerc_audio_t *device, player_msghdr_t *header, uint8_t *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_WAV_REC)) { player_audio_wav_t * wdata = (player_audio_wav_t *) data; assert(header->size > 0); device->wav_data.data_count = wdata->data_count; if (device->wav_data.data != NULL) free (device->wav_data.data); if ((device->wav_data.data = (uint8_t*) malloc (wdata->data_count)) == NULL) PLAYERC_ERR("Failed to allocate space to store wave data locally"); else { memcpy(device->wav_data.data, wdata->data, wdata->data_count * sizeof(device->wav_data.data[0])); device->wav_data.format = wdata->format; } } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_SEQ)) { player_audio_seq_t * sdata = (player_audio_seq_t *) data; assert(header->size > 0); device->seq_data.tones_count = sdata->tones_count; memcpy(device->seq_data.tones, sdata->tones, sdata->tones_count * sizeof(device->seq_data.tones[0])); } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_MIXER_CHANNEL)) { player_audio_mixer_channel_list_t * wdata = (player_audio_mixer_channel_list_t *) data; assert(header->size > 0); device->mixer_data.channels_count = wdata->channels_count; memcpy(device->mixer_data.channels, wdata->channels, wdata->channels_count * sizeof(device->mixer_data.channels[0])); } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_STATE)) { player_audio_state_t *sdata = (player_audio_state_t *) data; assert(header->size > 0); device->state = sdata->state; } else PLAYERC_WARN2("skipping audio message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// Process incoming data void playerc_wsn_putmsg (playerc_wsn_t *device, player_msghdr_t *header, void *data) { // int i, j; if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_WSN_DATA_STATE)) { player_wsn_data_t* wsn_data = (player_wsn_data_t*)data; device->node_type = wsn_data->node_type; device->node_id = wsn_data->node_id; device->node_parent_id = wsn_data->node_parent_id; device->data_packet = wsn_data->data_packet; } else PLAYERC_WARN2("skipping wsn message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
void playerc_actarray_putmsg(playerc_actarray_t *device, player_msghdr_t *header, player_actarray_data_t *data, size_t len) { uint32_t i = 0; if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_ACTARRAY_DATA_STATE)) { device->actuators_count = data->actuators_count; device->actuators_data = realloc(device->actuators_data,device->actuators_count*sizeof(device->actuators_data[0])); for (i = 0; i < device->actuators_count; i++) { device->actuators_data[i] = data->actuators[i]; } device->motor_state = data->motor_state; } else PLAYERC_WARN2("skipping actarray message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
void playerc_blinkenlight_putmsg(playerc_blinkenlight_t *device, player_msghdr_t *header, void* data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_BLINKENLIGHT_DATA_STATE)) { player_blinkenlight_data_t* blink = (player_blinkenlight_data_t*)data; device->enabled = blink->enable; device->duty_cycle = blink->dutycycle; device->period = blink->period; device->red = blink->color.red; device->green = blink->color.green; device->blue = blink->color.blue; } else PLAYERC_WARN2("skipping blinkenlight message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// Process incoming data void playerc_joystick_putmsg(playerc_joystick_t *device, player_msghdr_t *header, player_joystick_data_t *data, size_t len) { uint32_t i; if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_JOYSTICK_DATA_STATE)) { device->buttons = data->buttons; device->axes_count = data->axes_count; for (i = 0; i < data->axes_count ; i++) device->pos[i] = data->pos[i]; } else PLAYERC_WARN2("skipping joystick message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); return; }
// Process incoming data void playerc_health_putmsg (playerc_health_t *device, player_msghdr_t *header, player_health_data_t *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_HEALTH_DATA_STATE)) { device->cpu_usage.idle = data->cpu_usage.idle; device->cpu_usage.system = data->cpu_usage.system; device->cpu_usage.user = data->cpu_usage.user; device->mem.total = data->mem.total; device->mem.used = data->mem.used; device->mem.free = data->mem.free; device->swap.total = data->swap.total; device->swap.used = data->swap.used; device->swap.free = data->swap.free; } else PLAYERC_WARN2("skipping health message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
void playerc_limb_putmsg(playerc_limb_t *device, player_msghdr_t *header, player_limb_data_t *data, size_t len) { if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_LIMB_DATA_STATE)) { device->data.position.px = data->position.px; device->data.position.py = data->position.py; device->data.position.pz = data->position.pz; device->data.approach.px = data->approach.px; device->data.approach.py = data->approach.py; device->data.approach.pz = data->approach.pz; device->data.orientation.px = data->orientation.px; device->data.orientation.py = data->orientation.py; device->data.orientation.pz = data->orientation.pz; device->data.state = data->state; } else PLAYERC_WARN2("skipping limb message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// 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; }
// Process incoming data void playerc_laser_putmsg(playerc_laser_t *device, player_msghdr_t *header, void *data) { uint32_t i; double r, b, db; if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_LASER_DATA_SCAN)) { player_laser_data_t* scan_data = (player_laser_data_t*)data; b = scan_data->min_angle; db = scan_data->resolution; device->scan_start = b; device->scan_res = db; device->max_range = scan_data->max_range; device->min_left = device->max_range; device->min_right = device->max_range; device->scan_count = scan_data->ranges_count; playerc_laser_reallocate_scans(device); for (i = 0; i < scan_data->ranges_count; i++) { r = scan_data->ranges[i]; //assert(r >= 0); device->ranges[i] = r; device->scan[i][0] = r; device->scan[i][1] = b; device->point[i].px = r * cos(b); device->point[i].py = r * sin(b); b += db; if((i <= scan_data->ranges_count/2) && (r < device->min_right)) device->min_right = r; else if((i > scan_data->ranges_count/2) && (r < device->min_left)) device->min_left = r; } for (i = 0; i < scan_data->intensity_count; i++) device->intensity[i] = scan_data->intensity[i]; device->scan_id = scan_data->id; } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_LASER_DATA_SCANPOSE)) { player_laser_data_scanpose_t* scan_data = (player_laser_data_scanpose_t*)data; b = scan_data->scan.min_angle; db = scan_data->scan.resolution; device->scan_start = b; device->scan_res = db; device->scan_count = scan_data->scan.ranges_count; playerc_laser_reallocate_scans(device); for (i = 0; i < scan_data->scan.ranges_count; i++) { r = scan_data->scan.ranges[i]; assert(r >= 0); device->ranges[i] = r; device->scan[i][0] = r; device->scan[i][1] = b; device->point[i].px = r * cos(b); device->point[i].py = r * sin(b); b += db; } for (i = 0; i < scan_data->scan.intensity_count; i++) device->intensity[i] = scan_data->scan.intensity[i]; device->scan_id = scan_data->scan.id; device->robot_pose[0] = scan_data->pose.px; device->robot_pose[1] = scan_data->pose.py; device->robot_pose[2] = scan_data->pose.pa; } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_LASER_DATA_SCANANGLE)) { player_laser_data_scanangle_t* scan_data = (player_laser_data_scanangle_t*)data; device->max_range = scan_data->max_range; device->min_left = device->max_range; device->min_right = device->max_range; device->scan_count = scan_data->ranges_count; playerc_laser_reallocate_scans(device); for (i = 0; i < scan_data->ranges_count; i++) { r = scan_data->ranges[i]; assert(r >= 0); b = scan_data->angles[i]; device->ranges[i] = r; device->scan[i][0] = r; device->scan[i][1] = b; device->point[i].px = r * cos(b); device->point[i].py = r * sin(b); if((i <= scan_data->ranges_count/2) && (r < device->min_right)) device->min_right = r; else if((i > scan_data->ranges_count/2) && (r < device->min_left)) device->min_left = r; } for (i = 0; i < scan_data->intensity_count; i++) device->intensity[i] = scan_data->intensity[i]; device->scan_id = scan_data->id; } else PLAYERC_WARN2("skipping laser message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); }
// 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; }
// 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; } } }
// 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; }