From: Brian G. <ge...@us...> - 2006-09-22 14:55:01
|
Update of /cvsroot/playerstage/code/player/utils/playernav In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv26409 Modified Files: playernav.c Log Message: added ability to get pose from planner or localizer Index: playernav.c =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/playernav/playernav.c,v retrieving revision 1.49 retrieving revision 1.50 diff -C2 -d -r1.49 -r1.50 *** playernav.c 15 Sep 2006 17:53:49 -0000 1.49 --- playernav.c 22 Sep 2006 21:54:58 -0000 1.50 *************** *** 195,198 **** --- 195,201 ---- { + int have_robot = 0; + + // Try to get the pose from the planner if(gui_data->planners[i] && gui_data->planners[i]->info.fresh) { *************** *** 200,204 **** --- 203,222 ---- 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 >= *************** *** 226,233 **** (gui_data->robot_poses[i].pa != robot_pose.pa)) { - //printf("moving robot %d\n", i); move_item(gui_data->robot_items[i],robot_pose,1); ! //if(onmap && showparticlesp && gui_data->localizes[i]) if(showparticlesp && gui_data->localizes[i]) { --- 244,250 ---- (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]) { *************** *** 240,280 **** // regardless, store this pose for comparison on next iteration gui_data->robot_poses[i] = robot_pose; ! // 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); ! //gtk_main_quit(); ! //break; } else ! { ! //puts("drawing waypoints"); ! draw_waypoints(gui_data,i); ! } } - else - draw_goal(gui_data,i); - - 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; } ! gui_data->planners[i]->info.fresh = 0; ! //gui_data->localizes[i]->info.fresh = 0; } --- 257,303 ---- // 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; } |