// Subscribe to a device
int playerc_client_subscribe(playerc_client_t *client, int code, int index,
int access, char *drivername, size_t len)
{
player_device_req_t req, *resp;
resp=NULL;
req.addr.host = 0;
req.addr.robot = 0;
req.addr.interf = code;
req.addr.index = index;
req.access = access;
req.driver_name_count = 0;
if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEV,
(void*)&req, (void**)&resp) < 0)
{
PLAYERC_ERR("failed to get response");
return -1;
}
if (req.access != access)
{
PLAYERC_ERR2("requested [%d] access, but got [%d] access", access, req.access);
return -1;
}
// Copy the driver name
strncpy(drivername, resp->driver_name, len);
player_device_req_t_free(resp);
return 0;
}
// Get the list of waypoints. The writes the result into the proxy
// rather than returning it to the caller.
int playerc_planner_get_waypoints(playerc_planner_t *device)
{
int i;
int len;
player_planner_waypoints_req_t config;
memset(&config, 0, sizeof(config));
config.subtype = PLAYER_PLANNER_GET_WAYPOINTS_REQ;
len = playerc_client_request(device->info.client, &device->info,
&config, sizeof(config), &config,
sizeof(config));
if (len < 0)
return -1;
if (len == 0)
{
PLAYERC_ERR("got unexpected zero-length reply");
return -1;
}
device->waypoint_count = (int)ntohs(config.count);
for(i=0;i<device->waypoint_count;i++)
{
device->waypoints[i][0] = ((int)ntohl(config.waypoints[i].x)) / 1e3;
device->waypoints[i][1] = ((int)ntohl(config.waypoints[i].y)) / 1e3;
device->waypoints[i][2] = ((int)ntohl(config.waypoints[i].a)) * M_PI / 180;
}
return 0;
}
// Get the sonar geometry. The writes the result into the proxy
// rather than returning it to the caller.
int playerc_sonar_get_geom(playerc_sonar_t *device)
{
int i, len;
player_sonar_geom_t config;
config.subtype = PLAYER_SONAR_GET_GEOM_REQ;
len = playerc_client_request(device->info.client, &device->info,
&config, sizeof(config), &config, sizeof(config));
if (len < 0)
return -1;
if (len != sizeof(config))
{
PLAYERC_ERR2("reply has unexpected length (%d != %d)", len, sizeof(config));
return -1;
}
device->pose_count = htons(config.pose_count);
for (i = 0; i < device->pose_count; i++)
{
device->poses[i][0] = ((int16_t) ntohs(config.poses[i][0])) / 1000.0;
device->poses[i][1] = ((int16_t) ntohs(config.poses[i][1])) / 1000.0;
device->poses[i][2] = ((int16_t) ntohs(config.poses[i][2])) * M_PI / 180;
}
return 0;
}
// Get the laser configuration.
int
playerc_laser_get_config(playerc_laser_t *device,
double *min_angle,
double *max_angle,
double *resolution,
double *range_res,
unsigned char *intensity,
double *scanning_frequency)
{
player_laser_config_t *config;
if(playerc_client_request(device->info.client, &device->info,
PLAYER_LASER_REQ_GET_CONFIG,
NULL, (void**)&config) < 0)
return(-1);
*min_angle = device->scan_start = config->min_angle;
*max_angle = config->max_angle;
*resolution = config->resolution;
device->scan_res = *resolution;
*intensity = device->intensity_on = config->intensity;
*range_res = config->range_res;
*scanning_frequency = config->scanning_frequency;
device->range_res = *range_res;
device->max_range = config->max_range;
player_laser_config_t_free(config);
return 0;
}
// Ranger get config
int playerc_ranger_get_config(playerc_ranger_t *device, double *min_angle,
double *max_angle, double *angular_res,
double *min_range, double *max_range,
double *range_res, double *frequency)
{
player_ranger_config_t *config;
if(playerc_client_request(device->info.client, &device->info,
PLAYER_RANGER_REQ_GET_CONFIG,
NULL, (void**)&config) < 0)
return(-1);
playerc_ranger_copy_config(device, config);
player_ranger_config_t_free(config);
if (min_angle != NULL)
*min_angle = device->min_angle;
if (max_angle != NULL)
*max_angle = device->max_angle;
if (angular_res != NULL)
*angular_res = device->angular_res;
if (min_range != NULL)
*min_range = device->min_range;
if (max_range != NULL)
*max_range = device->max_range;
if (range_res != NULL)
*range_res = device->range_res;
if (frequency != NULL)
*frequency = device->frequency;
return 0;
}
// Configure the laser.
int
playerc_laser_set_config(playerc_laser_t *device,
double min_angle,
double max_angle,
double resolution,
double range_res,
unsigned char intensity,
double scanning_frequency)
{
player_laser_config_t config;
config.min_angle = min_angle;
config.max_angle = max_angle;
config.resolution = resolution;
config.intensity = (intensity ? 1 : 0);
config.range_res = range_res;
config.scanning_frequency = scanning_frequency;
if(playerc_client_request(device->info.client, &device->info,
PLAYER_LASER_REQ_SET_CONFIG,
(void*)&config, NULL) < 0)
return -1;
// if the set suceeded copy them locally
device->scan_start = config.min_angle;
device->scan_res = config.resolution;
device->range_res = config.range_res;
device->intensity_on = config.intensity;
device->scanning_frequency = config.scanning_frequency;
return 0;
}
// Set the robot pose (mean and covariance)
int playerc_localize_set_pose(playerc_localize_t *device, double pose[3], double cov[3][3])
{
int len;
player_localize_set_pose_t req;
req.subtype = PLAYER_LOCALIZE_SET_POSE_REQ;
req.mean[0] = htonl((int) (pose[0] * 1e3));
req.mean[1] = htonl((int) (pose[1] * 1e3));
req.mean[2] = htonl((int) (pose[2] * 180 / M_PI * 3600));
req.cov[0][0] = htonll((int64_t) (cov[0][0] * 1e6));
req.cov[0][1] = htonll((int64_t) (cov[0][1] * 1e6));
req.cov[0][2] = 0;
req.cov[1][0] = htonll((int64_t) (cov[1][0] * 1e6));
req.cov[1][1] = htonll((int64_t) (cov[1][1] * 1e6));
req.cov[1][2] = 0;
req.cov[2][0] = 0;
req.cov[2][1] = 0;
req.cov[2][2] = htonll((int64_t) (cov[2][2] * 180 / M_PI * 3600 * 180 / M_PI * 3600));
len = playerc_client_request(device->info.client, &device->info,
&req, sizeof(req), NULL, 0);
if (len < 0)
return -1;
return 0;
}
// Set the object pose.
int playerc_truth_set_pose(playerc_truth_t *device,
double px, double py, double pz,
double rx, double ry, double rz)
{
int len;
player_truth_pose_t config;
config.subtype = PLAYER_TRUTH_SET_POSE;
config.pos[0] = htonl((uint32_t) (int32_t) (px * 1000));
config.pos[1] = htonl((uint32_t) (int32_t) (py * 1000));
config.pos[2] = htonl((uint32_t) (int32_t) (pz * 1000));
config.rot[0] = htonl((uint32_t) (int32_t) (rx * 1000));
config.rot[1] = htonl((uint32_t) (int32_t) (ry * 1000));
config.rot[2] = htonl((uint32_t) (int32_t) (rz * 1000));
len = playerc_client_request(device->info.client, &device->info,
&config, sizeof(config), &config, sizeof(config));
if (len < 0)
return -1;
// TODO: check for a NACK
return 0;
}
// Unsubscribe from a device
int playerc_client_unsubscribe(playerc_client_t *client, int code, int index)
{
player_device_req_t req, *resp;
int ret;
req.addr.host = 0;
req.addr.robot = 0;
req.addr.interf = code;
req.addr.index = index;
req.access = PLAYER_CLOSE_MODE;
req.driver_name_count = 0;
if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEV,
(void*)&req, (void**)&resp) < 0)
return -1;
if (resp->access != PLAYER_CLOSE_MODE)
{
PLAYERC_ERR2("requested [%d] access, but got [%d] access", PLAYER_CLOSE_MODE, resp->access);
ret = -1;
}
else
{
ret = 0;
}
player_device_req_t_free(resp);
return ret;
}
/** @brief Request to retrieve an audio sample
Data is stored in wav_data */
int playerc_audio_sample_retrieve(playerc_audio_t *device, int index)
{
int result = 0;
player_audio_sample_t req;
player_audio_sample_t * resp;
req.sample.data_count = 0;
req.index = index;
if((result = playerc_client_request(device->info.client, &device->info,
PLAYER_AUDIO_REQ_SAMPLE_RETRIEVE,
&req, (void**)&resp)) < 0)
return result;
device->wav_data.data_count = resp->sample.data_count;
if (device->wav_data.data != NULL)
free (device->wav_data.data);
if ((device->wav_data.data = (uint8_t*) malloc (resp->sample.data_count)) == NULL)
{
player_audio_sample_t_free(resp);
PLAYERC_ERR("Failed to allocate space to store wave data locally");
return -1;
}
memcpy(device->wav_data.data, resp->sample.data, resp->sample.data_count * sizeof(device->wav_data.data[0]));
device->wav_data.format = resp->sample.format;
player_audio_sample_t_free(resp);
return 0;
}
// Get the particle set
int playerc_localize_get_particles(playerc_localize_t *device)
{
int i;
player_localize_get_particles_t *req;
if(playerc_client_request(device->info.client, &device->info,
PLAYER_LOCALIZE_REQ_GET_PARTICLES,
NULL, (void**) &req) < 0)
return -1;
device->mean[0] = req->mean.px;
device->mean[1] = req->mean.py;
device->mean[2] = req->mean.pa;
device->variance = req->variance;
device->num_particles = req->particles_count;
device->particles = realloc(device->particles,req->particles_count*sizeof(device->particles[0]));
for(i=0;i<device->num_particles;i++)
{
device->particles[i].pose[0] = req->particles[i].pose.px;
device->particles[i].pose[1] = req->particles[i].pose.py;
device->particles[i].pose[2] = req->particles[i].pose.pa;
device->particles[i].weight = req->particles[i].alpha;
}
player_localize_get_particles_t_free(req);
return 0;
}
// Get the driver info for all devices. The data is written into the
// proxy structure rather than returned to the caller.
int playerc_client_get_driverinfo(playerc_client_t *client)
{
int i;
player_device_driverinfo_t req, *resp;
for (i = 0; i < client->devinfo_count; i++)
{
memset(&req,0,sizeof(req));
req.addr = client->devinfos[i].addr;
if(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DRIVERINFO,
&req, (void**)&resp) < 0)
{
PLAYERC_ERR("failed to get response");
return(-1);
}
strncpy(client->devinfos[i].drivername, resp->driver_name,
resp->driver_name_count);
client->devinfos[i].drivername[resp->driver_name_count] = '\0';
player_device_driverinfo_t_free(resp);
}
return 0;
}
// Set the robot pose (mean and covariance)
int playerc_localize_set_pose(playerc_localize_t *device, double pose[3], double cov[6])
{
player_localize_set_pose_t req;
req.mean.px = pose[0];
req.mean.py = pose[1];
req.mean.pa = pose[2];
req.cov[0] = cov[0];
req.cov[1] = cov[1];
req.cov[2] = cov[2];
req.cov[3] = cov[3];
req.cov[4] = cov[4];
req.cov[5] = cov[5];
if(playerc_client_request(device->info.client,
&device->info,
PLAYER_LOCALIZE_REQ_SET_POSE,
&req, NULL) < 0)
{
PLAYERC_WARN1("%s\n", playerc_error_str());
return -1;
}
return 0;
}
// Get the object pose.
int playerc_truth_get_pose(playerc_truth_t *device,
double *px, double *py, double *pz,
double *rx, double *ry, double *rz)
{
int len;
player_truth_pose_t config;
config.subtype = PLAYER_TRUTH_GET_POSE;
len = playerc_client_request(device->info.client, &device->info,
&config, sizeof(config), &config, sizeof(config));
if (len < 0)
return -1;
if (len != sizeof(config))
{
PLAYERC_ERR2("reply has unexpected length (%d != %d)", len, sizeof(config));
return -1;
}
*px = ((int32_t) ntohl(config.pos[0])) / 1000.0;
*py = ((int32_t) ntohl(config.pos[1])) / 1000.0;
*pz = ((int32_t) ntohl(config.pos[2])) / 1000.0;
*rx = ((int32_t) ntohl(config.rot[0])) / 1000.0;
*ry = ((int32_t) ntohl(config.rot[1])) / 1000.0;
*rz = ((int32_t) ntohl(config.rot[2])) / 1000.0;
return 0;
}
// Get the current configuration.
int playerc_localize_get_config(playerc_localize_t *device,
player_localize_config_t *cfg)
{
int len;
player_localize_config_t config;
config.subtype = PLAYER_LOCALIZE_GET_CONFIG_REQ;
len = playerc_client_request(device->info.client, &device->info,
&config, sizeof(config), &config, sizeof(config));
if (len < 0)
return -1;
if (len != sizeof(config))
{
PLAYERC_ERR2("reply has unexpected length (%d != %d)", len, sizeof(config));
return -1;
}
// fill out the data field
cfg->subtype = PLAYER_LOCALIZE_GET_CONFIG_REQ;
cfg->num_particles = (uint32_t) ntohl(config.num_particles);
return 0;
}
// Query the capabilities of a device
int playerc_device_hascapability(playerc_device_t *device, uint32_t type, uint32_t subtype)
{
player_capabilities_req_t capreq;
capreq.type = type;
capreq.subtype = subtype;
return playerc_client_request(device->client, device, PLAYER_CAPABILTIES_REQ,
&capreq, NULL) >= 0 ? 1 : 0;
}
int playerc_localize_get_map_tile(playerc_localize_t *device)
{
int i, j;
int ni, nj;
int sx, sy;
int si, sj;
int len;
size_t reqlen;
//player_localize_map_info_t info;
player_localize_map_data_t data;
// Already loaded
if (device->map_tile_y >= device->map_size_y)
return -1;
// Tile size
sx = (int) sqrt(sizeof(data.data));
sy = sx;
assert(sx * sy < sizeof(data.data));
i = device->map_tile_x;
j = device->map_tile_y;
// Get the map data in tiles
si = MIN(sx, device->map_size_x - i);
sj = MIN(sy, device->map_size_y - j);
data.subtype = PLAYER_LOCALIZE_GET_MAP_DATA_REQ;
data.col = htonl(i);
data.row = htonl(j);
data.width = htonl(si);
data.height = htonl(sj);
reqlen = sizeof(data) - sizeof(data.data);
len = playerc_client_request(device->info.client, &device->info,
&data, reqlen, &data, sizeof(data));
if (len < 0)
return -1;
if (len < reqlen + si * sj)
{
PLAYERC_ERR2("reply has unexpected length (%d < %d)", len, reqlen + si * sj);
return -1;
}
for (nj = 0; nj < sj; nj++)
for (ni = 0; ni < si; ni++)
device->map_cells[(i + ni) + (j + nj) * device->map_size_x] = data.data[ni + nj * si];
device->map_tile_x += sx;
if (device->map_tile_x >= device->map_size_x)
{
device->map_tile_x = 0;
device->map_tile_y += sy;
}
return 0;
}
// Set the speed of the end effector for all subsequent movement commands
int playerc_limb_speed_config(playerc_limb_t *device, float speed)
{
player_limb_speed_req_t config;
config.speed = speed;
return playerc_client_request(device->info.client, &device->info,
PLAYER_LIMB_REQ_SPEED,
&config, NULL);
}
// Turn the power to the limb on or off
int playerc_limb_power(playerc_limb_t *device, uint32_t enable)
{
player_limb_power_req_t config;
config.value = enable;
return playerc_client_request(device->info.client, &device->info,
PLAYER_LIMB_REQ_POWER,
&config, NULL);
}
// Turn the brakes of all actuators in the array that have them on or off
int playerc_actarray_brakes(playerc_actarray_t *device, uint8_t enable)
{
player_actarray_brakes_config_t config;
config.value = enable;
return playerc_client_request(device->info.client, &device->info,
PLAYER_ACTARRAY_REQ_BRAKES,
&config, NULL);
}
// Turn the brakes of all actuators in the limb that have them on or off
int playerc_limb_brakes(playerc_limb_t *device, uint32_t enable)
{
player_limb_brakes_req_t config;
config.value = enable;
return playerc_client_request(device->info.client, &device->info,
PLAYER_LIMB_REQ_BRAKES,
&config, NULL);
}
// Get the particle set
int playerc_localize_get_particles(playerc_localize_t *device)
{
int len;
int i;
player_localize_get_particles_t* req;
assert(req = calloc(1,sizeof(player_localize_get_particles_t)));
req->subtype = PLAYER_LOCALIZE_GET_PARTICLES_REQ;
len = playerc_client_request(device->info.client, &device->info,
req, sizeof(req), req,
sizeof(player_localize_get_particles_t));
if (len < 0)
{
free(req);
return -1;
}
// TODO: better length checking here
// byteswap
device->mean[0] = ((int32_t)ntohl(req->mean[0])) / 1e3;
device->mean[1] = ((int32_t)ntohl(req->mean[1])) / 1e3;
device->mean[2] = (((int32_t)ntohl(req->mean[2])) / 3600.0) * M_PI/180.0;
device->variance = ((uint64_t)ntohll(req->variance)) / (1e3 * 1e3);
device->num_particles = (int32_t)ntohl(req->num_particles);
for(i=0;i<device->num_particles;i++)
{
if(i >= PLAYER_LOCALIZE_PARTICLES_MAX)
{
device->num_particles = i;
PLAYERC_WARN("too many particles");
break;
}
device->particles[i].pose[0] =
((int32_t)ntohl(req->particles[i].pose[0])) / 1e3;
device->particles[i].pose[1] =
((int32_t)ntohl(req->particles[i].pose[1])) / 1e3;
device->particles[i].pose[2] =
(((int32_t)ntohl(req->particles[i].pose[2])) / 3600.0) *
M_PI / 180.0;
device->particles[i].weight =
((uint32_t)ntohl(req->particles[i].alpha)) / 1e6;
}
free(req);
return 0;
}
int
playerc_motor_position_control(playerc_motor_t *device, int type)
{
player_motor_position_mode_req_t config;
config.value = type;
return(playerc_client_request(device->info.client, &device->info,
PLAYER_MOTOR_REQ_VELOCITY_MODE,
&config, NULL));
}
// Set the speed of a joint (-1 for all joints) for all subsequent movement commands
int playerc_actarray_accel_config(playerc_actarray_t *device, int joint, float accel)
{
player_actarray_accel_config_t config;
config.joint = joint;
config.accel = accel;
return playerc_client_request(device->info.client, &device->info,
PLAYER_ACTARRAY_REQ_ACCEL,
&config, NULL);
}
// Set the speed of a joint (-1 for all joints) for all subsequent movement commands
int playerc_actarray_speed_config(playerc_actarray_t *device, int joint, float speed)
{
player_actarray_speed_config_t config;
config.joint = joint;
config.speed = speed;
return playerc_client_request(device->info.client, &device->info,
PLAYER_ACTARRAY_REQ_SPEED,
&config, NULL);
}
int playerc_lightsensor_get_int_lights_count(playerc_lightsensor_t *device) {
int *geom;
if (playerc_client_request(device->info.client, &device->info,
PLAYER_LIGHTSENSOR_REQ_GET_COLOR, NULL, (void**) &geom) < 0)
return (-1);
device->lights_count = *geom;
free(geom);
return (0);
}
// Get the ir geometry. The writes the result into the proxy
// rather than returning it to the caller.
int playerc_ir_get_geom(playerc_ir_t *device)
{
player_ir_pose_t *geom;
int ret;
ret = playerc_client_request(device->info.client, &device->info,PLAYER_IR_REQ_POSE, NULL, (void**)&geom);
if (ret < 0)
return ret;
player_ir_pose_t_copy(&device->poses, geom);
player_ir_pose_t_free(geom);
return 0;
}
// Enable/disable the motors
int
playerc_position1d_enable(playerc_position1d_t *device, int enable)
{
player_position1d_power_config_t config;
config.state = enable;
return(playerc_client_request(device->info.client,
&device->info,
PLAYER_POSITION1D_REQ_MOTOR_POWER,
&config, NULL));
}
// Enable/disable the motors
int playerc_motor_enable(playerc_motor_t *device, int enable)
{
player_motor_power_config_t config;
memset(&config, 0, sizeof(config));
config.request = PLAYER_MOTOR_POWER_REQ;
config.value = enable;
return playerc_client_request(device->info.client, &device->info,
&config, sizeof(config),
&config, sizeof(config));
}
// Change position/velocity control
int playerc_motor_position_control(playerc_motor_t *device, int type)
{
player_motor_power_config_t config;
memset(&config, 0, sizeof(config));
config.request = PLAYER_MOTOR_VELOCITY_MODE_REQ;
config.value = type;
return playerc_client_request(device->info.client, &device->info,
&config, sizeof(config),
&config, sizeof(config));
}
