Go to a target position

Hello, 
I'm new to use player stage simulator. 
I should develop a robot trajectory. In a first step, my robot should move
to  predetermined target positions while avoiding obstacles using laser.
The function GoTo(position) works very well. However, the robot doesn't
avoid obstacles with this function. 
I tried to see in the examples of player and I found some functions like : 

To move robot to a target position : 
-------------------------------------------------------------
void
position_goto(player_pose2d_t target)
{
  using namespace PlayerCc;

  double dist, angle;

  dist = sqrt((target.px - pp->GetXPos())*
              (target.px - pp->GetXPos()) +
              (target.py - pp->GetYPos())*
              (target.py - pp->GetYPos()));

  angle = atan2(target.py - pp->GetYPos(),
                target.px - pp->GetXPos());

  double newturnrate = 0;
  double newspeed = 0;

  if (fabs(rtod(angle)) > 10.0)
  {
    newturnrate = limit((angle/M_PI) * 40.0, -40.0, 40.0);
    newturnrate = dtor(newturnrate);
  }
  else
    newturnrate = 0.0;

  if (dist > 0.05)
  {
    newspeed = limit(dist * 0.200, -0.2, 0.2);
  }
  else
    newspeed = 0.0;

  if (fabs(newspeed) < 0.01)
    gGotoDone = true;

}
--------------------------------------------------------------------------

To avoid obstacles : 
----------------------------------------------------------------------------
void
AvoidObstaclesByLaser(LaserProxy *lp, double avoidcount, bool  obs){     
 
        for (uint32_t i = 0; i < lp->GetCount (); i++)
        {

          if(((*lp)[i]) && (*lp)[i] < minfrontdistance)
	   obs = true;
	}   
	  
      if(obs || avoidcount)
      {
        newspeed = avoidspeed;

        /* once we start avoiding, continue avoiding for 2 seconds */
        /* (we run at about 10Hz, so 20 loop iterations is about 2 sec) */
        if(!avoidcount)
        {
          avoidcount = (1+(int)(10.0*rand()/(RAND_MAX+1.0))) + 20;

          if(gUseLaser)
          {
            if(lp->MinLeft () < lp->MinRight ())
	    {
	      newturnrate = -turnrate;	    
	    }
            else
	    {
              newturnrate = turnrate;	      	           
	    }
          }
         
        }
         avoidcount--;
      }
      else
      {
        avoidcount = 0;  
	newturnrate = 0.0f;     
      } 
}
---------------------------------------------------------------------

In the main  I call the two functions:

-------------------------------------------------------------------

pp.SetMotorEnable (true);   
    newspeed = 0.0;
    newturnrate = 0.0;
    for(;;)
    {
       robot.Read();   
      AvoidObstaclesByLaser( lp,  avoidcount, obs);
      position_goto(tab_target[0]);      
      pp.SetSpeed(newspeed, newturnrate);    
    }
------------------------------------------------------------------------

Unfortunately, using this code the robot doesn't move to the target
position. 

Someone can help me? 

Thanks,
Ines


  



--
View this message in context: http://player-stage-gazebo.10965.n7.nabble.com/Go-to-a-target-position-tp19187.html
Sent from the playerstage-developers mailing list archive at Nabble.com.

------------------------------------------------------------------------------
Android apps run on BlackBerry 10
Introducing the new BlackBerry 10.2.1 Runtime for Android apps.
Now with support for Jelly Bean, Bluetooth, Mapview and more.
Get your Android app in front of a whole new audience.  Start now.
http://pubads.g.doubleclick.net/gampad/clk?id=124407151&iu=/4140/ostg.clktrk