// Destroy a map device
void map_destroy(map_t *map)
{
if (map->proxy->info.subscribed)
playerc_map_unsubscribe(map->proxy);
playerc_map_destroy(map->proxy);
rtk_fig_destroy(map->fig);
free(map->drivername);
free(map);
return;
}
void
fini_player(playerc_mclient_t* mclient,
playerc_client_t** clients,
playerc_map_t** maps,
playerc_localize_t** localizes,
playerc_planner_t** planners,
int num_bots)
{
int i;
for(i=0;i<num_bots;i++)
{
if(maps[i])
playerc_map_destroy(maps[i]);
if(localizes[i])
playerc_localize_destroy(localizes[i]);
if(planners[i])
playerc_planner_destroy(planners[i]);
playerc_client_destroy(clients[i]);
}
playerc_mclient_destroy(mclient);
}
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;
}
