// Basic localize test
int test_localize(playerc_client_t *client, int index)
{
int t, i;
void *rdevice;
playerc_localize_t *device;
//double min, max;
//int resolution, intensity;
printf("device [localize] index [%d]\n", index);
device = playerc_localize_create(client, index);
TEST("subscribing (read)");
if (playerc_localize_subscribe(device, PLAYER_READ_MODE) == 0)
PASS();
else
{
FAIL();
return(-1);
}
// deprecated: get map from map interface now
#if 0
TEST("get map");
if (playerc_localize_get_map(device) == 0)
PASS();
else
FAIL();
#endif
/* FIX
TEST("set configuration");
min = -M_PI/2;
max = +M_PI/2;
resolution = 100;
intensity = 1;
if (playerc_localize_set_config(device, min, max, resolution, intensity) == 0)
PASS();
else
FAIL();
TEST("get configuration");
if (playerc_localize_get_config(device, &min, &max, &resolution, &intensity) == 0)
PASS();
else
FAIL();
TEST("check configuration sanity");
if (abs(min + M_PI/2) > 0.01 || abs(max - M_PI/2) > 0.01)
FAIL();
else if (resolution != 100 || intensity != 1)
FAIL();
else
PASS();
*/
for (t = 0; t < 10; t++)
{
TEST1("reading data (attempt %d)", t);
do
rdevice = playerc_client_read(client);
while (rdevice == client);
if (rdevice == device)
{
PASS();
printf("localize: [%d %14.3f] [%d] ",
device->pending_count, device->pending_time, device->hypoth_count);
for (i = 0; i < device->hypoth_count; i++)
printf("[%6.3f, %6.3f %6.3f] ",
device->hypoths[i].mean[0], device->hypoths[i].mean[1], device->hypoths[i].mean[2]);
printf("\n");
}
else
{
FAIL();
break;
}
}
TEST("unsubscribing");
if (playerc_localize_unsubscribe(device) == 0)
PASS();
else
FAIL();
playerc_localize_destroy(device);
return 0;
}
/*
* Connect to player at each host:port pair, as specified by the global
* vars 'hostnames' and 'ports'. Also subscribes to each device, and adds
* each client into a new multiclient (which is returned)
*/
playerc_mclient_t*
init_player(playerc_client_t** clients,
playerc_map_t** maps,
playerc_localize_t** localizes,
playerc_planner_t** planners,
int num_bots,
char** hostnames,
int* ports,
int data_freq,
int map_idx,
int planner_idx)
{
int i;
playerc_mclient_t* mclient;
/* Connect to Player */
assert(mclient = playerc_mclient_create());
for(i=0; i<num_bots; i++)
{
assert(clients[i] =
playerc_client_create(mclient, hostnames[i], ports[i]));
if(playerc_client_connect(clients[i]) < 0)
{
fprintf(stderr, "Failed to connect to %s:%d\n",
hostnames[i], ports[i]);
return(NULL);
}
#if 0
if(playerc_client_datafreq(clients[i],data_freq) < 0)
{
fprintf(stderr, "Failed to set data frequency\n");
return(NULL);
}
// request ALL data, rather than just NEW data, because the localizer
// may only send out updates occasionally.
if(playerc_client_datamode(clients[i],PLAYER_DATAMODE_PUSH_ALL) < 0)
{
fprintf(stderr, "Failed to set data mode\n");
return(NULL);
}
#endif
// only subscribe to the first robot's map
if(i==0)
{
assert(maps[i] = playerc_map_create(clients[i], map_idx));
if(playerc_map_subscribe(maps[i],PLAYER_OPEN_MODE) < 0)
{
fprintf(stderr, "Failed to subscribe to map\n");
return(NULL);
}
}
else
maps[i] = NULL;
assert(localizes[i] = playerc_localize_create(clients[i], 0));
if(playerc_localize_subscribe(localizes[i],PLAYER_OPEN_MODE) < 0)
{
fprintf(stderr, "Warning: Failed to subscribe to localize on robot %d; you won't be able to set its pose.\n",i);
playerc_localize_destroy(localizes[i]);
localizes[i] = NULL;
}
assert(planners[i] = playerc_planner_create(clients[i], planner_idx));
if(playerc_planner_subscribe(planners[i],PLAYER_OPEN_MODE) < 0)
{
fprintf(stderr, "Warning: Failed to subscribe to planner on robot %d; you won't be able to give it goals.\n",i);
playerc_planner_destroy(planners[i]);
planners[i] = NULL;
}
}
//playerc_map_unsubscribe(maps[0]);
#if 0
/* Get at least one round of data from each robot */
for(;;)
{
if(playerc_mclient_read(mclient,-1) < 0)
{
fprintf(stderr, "Error on read\n");
return(NULL);
}
for(i=0; i<num_bots; i++)
{
if(!truths[i]->info.fresh ||
!lasers[i]->info.fresh ||
!planners[i]->info.fresh)
break;
}
if(i==num_bots)
break;
}
#endif
return(mclient);
}
