Quantcast

Go to a target position

classic Classic list List threaded Threaded
1 message Options
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Go to a target position

Ines
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


 
Loading...