void draw_all( void )
{
int i;
glClear( GL_COLOR_BUFFER_BIT );
draw_goal( goal );
for ( i = 0; i < params.obstacle_number; ++i )
{
draw_obstacle( obstacles[i] );
}
if ( show_connectivity ) { draw_agent_connectivity(); }
for( i = 0; i < params.agent_number; ++i )
{
draw_agent( agents[i] );
}
draw_params_stats();
draw_instructions();
glutSwapBuffers();
}
static void my_draw( Viewer *viewer, Renderer *renderer )
{
RendererGoal *self = (RendererGoal*) renderer->user;
for (int i = 0; i < self->recv_goal_lists->len; i++) {
lcmtypes_goal_list_t * l = g_ptr_array_index (self->recv_goal_lists, i);
for (int j = 0; j < l->num_goals; j++) {
lcmtypes_goal_t * goal = l->goals + j;
int edit_idx = -1;
int hover_idx = -1;
if (l->sender_id == self->sender_id && self->edit_goal)
edit_idx = self->edit_goal - &g_array_index (self->my_goals,
lcmtypes_goal_t, 0);
if (l->sender_id == self->sender_id && self->hover_goal)
hover_idx = self->hover_goal - &g_array_index (self->my_goals,
lcmtypes_goal_t, 0);
if (j == edit_idx)
glColor4f(.5, 1, .5, .5);
else if (self->ehandler.hovering && j == hover_idx)
glColor4f(.5, .8, .5, .5);
else
glColor4f(0, .5, .5, 0.3);
draw_goal (self, goal);
}
}
}
/// Draws the stairs with angle given around the y-axis
void stair::draw(vector3f angle)
{
gfx_push_matrix();
gfx_color3f(1, 1, 1);
gfx_translatef(ginfo->pos->x, ginfo->pos->y, ginfo->pos->z);
gfx_rotatef(this->angle, 0, 1, 0);
draw_stairs();
gfx_pop_matrix();
draw_goal(angle);
}
// 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);
}
