Hi,

I could get the planner driver working. But the robot is not able to find the track in the given map.

I would get this message  in the stage terminal :

new goal: 0.000000, 3.000000, 0.000000
Wavefront: global plan failed
Wavefront (port 6665):
No path from (-7.000,-7.000,45.000) to (0.000,3.000,0.000)

I am using the cave.png image . There is a path that is easily possible. So I was thinking where I am going wrong. 


The program:

 
#include <iostream>
#include <libplayerc++/playerc++.h>
#define pi 3.1415926535




using namespace PlayerCc;
using namespace std;



int main(int argc, char *argv[])
{
  

  PlayerClient    robot("localhost");
  SonarProxy      sp(&robot,0);
  Position2dProxy pp(&robot,0);
  Position2dProxy nd(&robot,2);
  PlannerProxy    plannerp(&robot,0); 
  
  
  
  
  player_pose2d_t present_pos,final_pos,vel_pos;
  
  int i;
  double temp_px,temp_py,temp_pa,temp,plannerp_GetPathValid;//temp_px,temp_py,temp_pa is used to read the value of present position of robot. 
  
  
  
  present_pos.px = -7;
  present_pos.py = -7;
  present_pos.pa = dtor(45);
 
  //If you want to the user to enter the present position. 
           
  //cout<<"Enter the Present start  postion";
  //cin>>present_pos.px>>present_pos.py>>present_pos.pa;
 
  nd.SetOdometry (present_pos.px,present_pos.py,present_pos.pa);
  pp.SetOdometry (present_pos.px,present_pos.py,present_pos.pa);
  
  
  // Update the client program with the data on the server
  robot.Read();
  robot.Read();
  robot.Read();
  robot.Read(); 

  temp_px = pp.GetXPos ();
  temp_py = pp.GetYPos ();
  temp_pa = pp.GetYaw ();

  cout<<temp_px<<"  "<<temp_py<<"  "<<temp_pa<<endl; 

  
  cout<<"Enter the Final postion";
  cin>>final_pos.px>>final_pos.py>>final_pos.pa;

  cout<<"Finale x="<<final_pos.px<<"Final y="<<final_pos.py<<"Finale z = "<<final_pos.pa<<endl; 

  plannerp.SetGoalPose (final_pos.px,final_pos.py,final_pos.pa);
 
/*
  while(1)
  {
  
   
   // Update the client program with the data on the server
robot.Read();
temp_px = pp.GetXPos ();
temp_py = pp.GetYPos ();
temp_pa = pp.GetYaw ();

cout<<temp_px<<"  "<<temp_py<<"  "<<temp_pa<<endl;
  
  }  

*/
  // Code to look into the details of the planner algorithm

plannerp_GetPathValid= plannerp.GetPathValid();
cout<<" plannerp.GetPathValid  = "<<plannerp_GetPathValid<<"\n";

 if (plannerp.GetPathValid ())
 {
 cout<<"On the way \n";
 while( !(plannerp.GetPathDone ()) )
 {
       cout<<"In while loop\n";
 }
       
 
 plannerp.RequestWaypoints ();

 cout<<"Number of way point "<<plannerp.GetWaypointCount ()<<"\n";

 for (i=0 ;i<plannerp.GetWaypointCount ();i++)
 {
cout<<"The value of "<<i<<"Planner is"<<plannerp[i]<<endl<<"\n";
 }
   
cout<<"Reached";  

}
 
else
{
cout<<"No valid path \n";
}
 

while(1);



}



CFG File 



# load the Stage plugin simulation driver
driver
(
  name "stage"
  provides ["simulation:0" ]
  plugin "libstageplugin"

  # load the named file into the simulator
  worldfile "simple.world"
)

# Export the map
driver
(
  name "stage"
  provides ["map:0" ]
  model "cave"
)

# Create a Stage driver and attach position2d and laser interfaces 
# to the model "robot1"
driver
  name "stage"
  provides ["position2d:0" "laser:0" "sonar:0" ]
  model "robot1" 
)

# Demonstrates use of a Player "abstract driver": one that doesn't
# interface directly with hardware, but only with other Player devices.
# The VFH driver attempts to drive to commanded positions without 
# bumping into obstacles.
driver 
(
  name "vfh"
  provides ["position2d:1"]
  requires ["position2d:0" "laser:0" ]
)


#driver for ND obstable avoidance
driver
(
  name "nd"
  provides ["position2d:2"]
  requires ["output:::position2d:0" "input:::position2d:0" "laser:0" "sonar:0" ]

  max_speed [0.3 30.0]
  min_speed [0.1 10.0]
  goal_tol [0.3 15.0]
  wait_on_stall 1

  rotate_stuck_time 5.0
  translate_stuck_time 5.0
  translate_stuck_dist 0.15
  translate_stuck_angle 10.0

  avoid_dist 0.4
  safety_dist 0.0

  laser_buffer 1
  
)

driver
(
  name "mapfile"
  provides ["map:1"]
  filename "bitmaps/cave.png"
  resolution 0.1
)


driver
(
  name "wavefront"
  provides ["planner:0"]
  requires ["output:::position2d:2" "input:::position2d:0" "map:1"]
 # safety_dist 0.15
 # distance_epsilon 0.5
 # angle_epsilon 10
)





Regards,
Suraj Swami