// 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_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_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_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_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);
}
// 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);
}
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);
}
// 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;
}
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);
}
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);
}
// 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);
}
// 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);
}
// Disconnect from the server, with potential retry
int playerc_client_disconnect_retry(playerc_client_t *client)
{
int retval;
int i;
int j;
struct timespec sleeptime;
sleeptime.tv_sec = (long) client->retry_time;
sleeptime.tv_nsec = 0;
/* Disconnect */
if((retval = playerc_client_disconnect(client)) != 0)
PLAYERC_WARN("playerc_client_disconnect() failed");
for(j=0; (client->retry_limit < 0) || (j<client->retry_limit); j++)
{
PLAYERC_WARN1("Reconnecting, attempt %d", j);
/* Reconnect */
if((retval = playerc_client_connect(client)) != 0)
PLAYERC_WARN("playerc_client_connect() failed");
else
{
/* Clean out buffers */
client->read_xdrdata_len = 0;
/* TODO: re-establish replacement rules, delivery modes, etc. */
/* Re-subscribe to devices */
for(i=0;i<client->device_count;i++)
{
if(client->device[i]->subscribed)
{
// TODO: what should access be here?
if((retval = playerc_device_subscribe(client->device[i],
PLAYERC_OPEN_MODE)) != 0)
{
PLAYERC_WARN2("playerc_device_subscribe() failed for %d:%d",
client->device[i]->addr.interf,
client->device[i]->addr.index);
// TODO: Subscription failed for one device; should we give up?
if(playerc_client_disconnect(client) != 0)
PLAYERC_WARN("playerc_client_disconnect() failed");
break;
}
}
}
// Did we get all of them?
if(i == client->device_count)
break;
}
nanosleep(&sleeptime,NULL);
}
if((client->retry_limit < 0) || (j < client->retry_limit))
{
PLAYERC_WARN("successfully reconnected");
return(0);
}
else
{
PLAYERC_WARN("failed to reconnect");
return(-1);
}
}