int
main(int argc, const char **argv)
{
int i;
// A service discovery object
player_sd_t* sd;
// Initialize multiclient
mclient = playerc_mclient_create();
// Initialize service discovery
sd = player_sd_init();
// Look for Player devices
if(player_sd_browse(sd, 0.0, 1, browse_cb) != 0)
{
puts("player_sd_browse error");
exit(-1);
}
for(;;)
{
// Update name service
player_sd_update(sd,0.0);
// Wait for new data from server
playerc_mclient_read(mclient,10);
}
// Shutdown
for(i=0;i<num_laserdevs;i++)
{
playerc_laser_unsubscribe(lasers[i]);
playerc_laser_destroy(lasers[i]);
playerc_client_disconnect(clients[i]);
playerc_client_destroy(clients[i]);
}
playerc_mclient_destroy(mclient);
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);
}
// Function to read from player; will be called when GTK is idle
gboolean
player_read_func(gpointer* arg)
{
int i;
static int count=0;
pose_t robot_pose;
gui_data_t* gui_data = (gui_data_t*)arg;
static struct timeval lastdump = {0, 0};
static struct timeval lastmapupdate = {0, 0};
static player_pose2d_t lastwaypt[MAX_NUM_ROBOTS];
static player_pose2d_t lastgoal[MAX_NUM_ROBOTS];
struct timeval curr;
double diff;
gboolean onmap;
int peek;
if(!count)
{
memset(lastwaypt,0,sizeof(lastwaypt));
memset(lastgoal,0,sizeof(lastgoal));
}
// update map
if(mapupdatefreq)
{
gettimeofday(&curr,NULL);
diff = (curr.tv_sec + curr.tv_usec/1e6) -
(lastmapupdate.tv_sec + lastmapupdate.tv_usec/1e6);
if(diff >= 1.0/mapupdatefreq)
{
update_map(gui_data);
lastmapupdate = curr;
}
}
peek = playerc_mclient_peek(gui_data->mclient,10);
if(peek < 0)
{
//fprintf(stderr, "Error on peek\n");
//gtk_main_quit();
return(TRUE);
}
else if(peek == 0)
return(TRUE);
// read new data
if(playerc_mclient_read(gui_data->mclient,-1) <= 0)
{
//fprintf(stderr, "Error on read\n");
//gtk_main_quit();
return(TRUE);
}
for(i=0;i<gui_data->num_robots;i++)
{
int have_robot = 0;
// Try to get the pose from the planner
if(gui_data->planners[i] && gui_data->planners[i]->info.fresh)
{
robot_pose.px = gui_data->planners[i]->px;
robot_pose.py = gui_data->planners[i]->py;
robot_pose.pa = gui_data->planners[i]->pa;
have_robot = 1;
}
// Fall back on getting the pose from the localizer
else if(gui_data->localizes[i] &&
gui_data->localizes[i]->info.fresh &&
gui_data->localizes[i]->hypoth_count)
{
robot_pose.px = gui_data->localizes[i]->hypoths[0].mean.px;
robot_pose.py = gui_data->localizes[i]->hypoths[0].mean.py;
robot_pose.pa = gui_data->localizes[i]->hypoths[0].mean.pa;
have_robot = 1;
}
// If we got the robot's pose from somewhere
if(have_robot)
{
// is the robot localized within the map?
onmap = (robot_pose.px >=
gui_data->mapdev->origin[0]) &&
(robot_pose.px <
(gui_data->mapdev->origin[0] +
(gui_data->mapdev->width * gui_data->mapdev->resolution))) &&
(robot_pose.py >=
gui_data->mapdev->origin[1]) &&
(robot_pose.py <
(gui_data->mapdev->origin[1] +
(gui_data->mapdev->height * gui_data->mapdev->resolution)));
// if it's off the map, put it in the middle
if(!onmap)
robot_pose.px = robot_pose.py = 0.0;
// don't draw the new pose if the user is in the middle of moving the
// robot
if(!robot_moving_p || (robot_moving_idx != i))
{
// also don't draw it if the pose hasn't changed since last time
if((gui_data->robot_poses[i].px != robot_pose.px) ||
(gui_data->robot_poses[i].py != robot_pose.py) ||
(gui_data->robot_poses[i].pa != robot_pose.pa))
{
move_item(gui_data->robot_items[i],robot_pose,1);
// If we have a localizer, retrieve and draw particle cloud
if(showparticlesp && gui_data->localizes[i])
{
playerc_localize_get_particles(gui_data->localizes[i]);
draw_particles(gui_data,i);
}
// If we have a localizer retrive and draw uncertainty ellipsis
if(showuncertaintyp && gui_data->localizes[i])
{
draw_uncertainty(gui_data,i);
}
}
}
// regardless, store this pose for comparison on next iteration
gui_data->robot_poses[i] = robot_pose;
// If we have a planner
if(gui_data->planners[i])
{
// If the current goal or waypoint changed, get the whole list
// of waypoints again
if((lastwaypt[i].px != gui_data->planners[i]->wx) ||
(lastwaypt[i].py != gui_data->planners[i]->wy) ||
(lastwaypt[i].pa != gui_data->planners[i]->wa) ||
(lastgoal[i].px != gui_data->planners[i]->gx) ||
(lastgoal[i].py != gui_data->planners[i]->gy) ||
(lastgoal[i].pa != gui_data->planners[i]->ga))
{
if(get_waypoints)
{
if(playerc_planner_get_waypoints(gui_data->planners[i]) < 0)
{
fprintf(stderr,
"error while getting waypoints for robot %d\n", i);
}
else
{
draw_waypoints(gui_data,i);
}
}
else
draw_goal(gui_data,i);
// Cache goal and waypoint info
lastwaypt[i].px = gui_data->planners[i]->wx;
lastwaypt[i].py = gui_data->planners[i]->wy;
lastwaypt[i].pa = gui_data->planners[i]->wa;
lastgoal[i].px = gui_data->planners[i]->gx;
lastgoal[i].py = gui_data->planners[i]->gy;
lastgoal[i].pa = gui_data->planners[i]->ga;
}
}
// Reset freshness flag(s)
if(gui_data->planners[i])
gui_data->planners[i]->info.fresh = 0;
if(gui_data->localizes[i])
gui_data->localizes[i]->info.fresh = 0;
}
// raise the robot's canvas item, so that the user can select it
if(gui_data->localizes[i] || gui_data->planners[i])
{
gnome_canvas_item_raise_to_top(gui_data->robot_items[i]);
}
}
// dump screenshot
if(dumpp)
{
gettimeofday(&curr,NULL);
diff = (curr.tv_sec + curr.tv_usec/1e6) -
(lastdump.tv_sec + lastdump.tv_usec/1e6);
if(diff >= 1.0/dumpfreq)
{
dump_screenshot(gui_data);
lastdump = curr;
}
}
count++;
return(TRUE);
}
