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