Update of /cvsroot/playerstage/code/player/utils/playernav
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv5544
Modified Files:
Tag: release-2-0-patches
playernav.c
Log Message:
merged changes from HEAD to allow pose to be taken from either localize or
planner
Index: playernav.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playernav/playernav.c,v
retrieving revision 1.47.2.2
retrieving revision 1.47.2.3
diff -C2 -d -r1.47.2.2 -r1.47.2.3
*** playernav.c 15 Sep 2006 17:54:30 -0000 1.47.2.2
--- playernav.c 22 Sep 2006 22:23:26 -0000 1.47.2.3
***************
*** 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,286 ****
// 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;
}
// raise the robot's canvas item, so that the user can select it
! //if(gui_data->localizes[i])
! if(gui_data->planners[i])
gnome_canvas_item_raise_to_top(gui_data->robot_items[i]);
}
--- 257,310 ----
// 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]);
+ }
}
***************
*** 397,402 ****
for(i=0;i<gui_data.num_robots;i++)
{
! //if(gui_data.localizes[i])
! if(gui_data.planners[i])
{
robot_pose.px = 0.0;
--- 421,425 ----
for(i=0;i<gui_data.num_robots;i++)
{
! if(gui_data.localizes[i] || gui_data.planners[i])
{
robot_pose.px = 0.0;
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys -- and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
