[Playerstage-commit] code/player/utils/playernav playernav.c, 1.47.2.2, 1.47.2.3

[Brian G. <ge...@us...>]

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;