// Update a map device
void map_update(map_t *map)
{
// Update the device subscription
if (rtk_menuitem_ischecked(map->subscribe_item))
{
if (!map->proxy->info.subscribed)
{
if (playerc_map_subscribe(map->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
// download a map
if (playerc_map_get_map( map->proxy ) >= 0)
map_draw( map );
}
}
else
{
if (map->proxy->info.subscribed)
if (playerc_map_unsubscribe(map->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
rtk_menuitem_check(map->subscribe_item, map->proxy->info.subscribed);
if (map->proxy->info.subscribed)
{
if(rtk_menuitem_ischecked(map->continuous_item))
{
static struct timeval old_time = {0,0};
struct timeval time;
double time_diff = 0.0;
gettimeofday(&time, NULL);
// i don't use (map->proxy->info.datatime != map->datatime))
// because some drivers return strange datatimes
// and the map may change to often for the current map format
// in playerv.h you can adjust MAP_UPDATE_TIME (default 1 sec)
time_diff = (double)(time.tv_sec - old_time.tv_sec) +
(double)(time.tv_usec - old_time.tv_usec)/1000000;
if (time_diff > MAP_UPDATE_TIME)
{
playerc_map_get_map(map->proxy);
map_draw(map);
old_time = time;
}
}
}
else
{
// Dont draw the map.
rtk_fig_show(map->fig, 0);
}
}
int main(int argc, char **argv)
{
int i;
playerc_client_t *client;
playerc_map_t *map;
FILE* fp;
host = "localhost";
port = 6665;
omap = 1;
idx = 0;
if(parse_args(argc,argv) < 0)
{
puts(USAGE);
exit(-1);
}
// Create a client and connect it to the server.
client = playerc_client_create(NULL, host, port);
if (playerc_client_connect(client) != 0)
return -1;
// Create and subscribe to a map device.
map = playerc_map_create(client, idx);
if (playerc_map_subscribe(map, PLAYER_OPEN_MODE))
return -1;
if(omap)
{
// Get the occ grid map
if (playerc_map_get_map(map) != 0)
return -1;
printf("map: %d X %d @ %.3f origin: %f %f \n",
map->width, map->height,
map->resolution,
map->origin[0], map->origin[1]);
create_map_image(map, fname, "png");
}
else
{
// Get the vector map
if (playerc_map_get_vector(map) != 0)
return -1;
if(!(fp = fopen(fname,"w+")))
{
perror("fopen() failed");
exit(-1);
}
fprintf(fp,"# Created by benson's SfLineScan.\n");
fprintf(fp,"origin %.0f %.0f\n", map->vminx * 1e3, map->vminy * 1e3);
fprintf(fp,"width %.0f\n", (map->vmaxx - map->vminx) * 1e3);
fprintf(fp,"height %.0f\n", (map->vmaxy - map->vminy) * 1e3);
for (i = 0; i < map->num_segments; i++)
{
fprintf(fp,"%.0f %.0f %.0f %.0f\n",
1e3*map->segments[i].x0,
1e3*map->segments[i].y0,
1e3*map->segments[i].x1,
1e3*map->segments[i].y1);
}
fclose(fp);
}
// Shutdown
playerc_map_unsubscribe(map);
playerc_map_destroy(map);
playerc_client_disconnect(client);
playerc_client_destroy(client);
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);
}
