void
update_map(gui_data_t* gui_data)
{
/* Get the map from the first robot */
//puts("requesting map");
if(playerc_map_get_map(gui_data->maps[0]) < 0)
{
fprintf(stderr, "Failed to get map\n");
}
else
{
//puts("done");
if(gui_data->imageitem)
gtk_object_destroy(GTK_OBJECT(gui_data->imageitem));
create_map_image(gui_data);
}
}
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;
}
int
main(int argc, char** argv)
{
int i;
int map_idx;
int planner_idx;
int have_map;
pose_t robot_pose;
gui_data_t gui_data;
memset(&gui_data, 0, sizeof(gui_data));
dumpfreq = 5.0;
gui_data.initial_zoom = 1.0;
gui_data.aa = 1;
map_idx = 0;
planner_idx = 0;
if(parse_args(argc-1, argv+1, &(gui_data.num_robots), gui_data.hostnames,
gui_data.ports, &(gui_data.initial_zoom), &(gui_data.aa),
&map_idx, &planner_idx) < 0)
{
// Input problem, print usage and exit with error
puts(USAGE);
exit(-1);
}
if (signal(SIGINT, _interrupt_callback) ==SIG_ERR)
{
// Couldn't register signal callback, exit with error
exit(-1);
}
gui_data.mclient = init_player(gui_data.clients,
gui_data.maps,
gui_data.localizes,
gui_data.planners,
gui_data.num_robots,
gui_data.hostnames,
gui_data.ports,
DATA_FREQ,
map_idx,
planner_idx);
if(! gui_data.mclient )
{
// Connection to Player failed, exit with error
exit(-1);
}
// assume the robots all start enabled (should really get the current
// enable/disable state for each robot from the server).
for(i=0;i<gui_data.num_robots;i++)
gui_data.robot_enable_states[i] = 1;
// use the first robot for the map
gui_data.mapdev = gui_data.maps[0];
/* Get the map, just so we know how big to make the window */
puts("requesting map");
if(playerc_map_get_map(gui_data.mapdev) < 0)
{
have_map = 0;
fprintf(stderr, "Failed to get map\n");
// no map yet; guess some values to initialize the GUI
gui_data.mapdev->width = gui_data.mapdev->height = 400;
gui_data.mapdev->resolution = 0.1;
gui_data.mapdev->origin[0] = -20.0;
gui_data.mapdev->origin[1] = -20.0;
}
else
{
have_map = 1;
puts("done");
}
init_gui(&gui_data, argc, argv);
// now draw the map
if(have_map)
create_map_image(&gui_data);
for(i=0;i<gui_data.num_robots;i++)
{
if(gui_data.localizes[i] || gui_data.planners[i])
{
robot_pose.px = 0.0;
robot_pose.py = 0.0;
robot_pose.pa = 0.0;
create_robot(&gui_data, i, robot_pose);
}
}
gtk_widget_show((GtkWidget*)(gui_data.main_window));
// setup read function to be called when idle
g_idle_add((GSourceFunc)player_read_func,
(gpointer*)&gui_data);
gtk_main();
fini_player(gui_data.mclient,
gui_data.clients,
gui_data.maps,
gui_data.localizes,
gui_data.planners,
gui_data.num_robots);
fini_gui(&gui_data);
return(0);
}
