move robot backwards

[marquesghm]

Hi,

I am modelling a robot that moves backwards using Goto function, however, it does not seem to do it. It always turn the robot toward the goal (with an angular move of 180o) and then it moves forwards.

Any suggestion of how to perform this movement ? Currently I am using the following code, but the error in the final position is quite large. I think there should be a better way to do it ....

best regards,

int DonnieClient::moveBackward(float arg)
{
    vector<float> path;

    robot->Read();
    double yaw = p2dProxy->GetYaw();    //Angulo do robo
    double posxi = p2dProxy->GetXPos();   //Posicao inicial X do robo
    double posyi = p2dProxy->GetYPos();   //Posicao inicial Y do robo

    PathNodes Point;

    for(int i = 1; i <= (int)arg; i++)
    {

      path.push_back(i*STEP_LENGHT);
    }

    float fltPart = arg - (int)arg;

    if(fltPart > 0)
    {
      path.push_back(fltPart);
    }

    float Npassos = arg;
    int passos = 0;

    bool obstacle = false;
    bool stop = true;
    float erro;
    float andou;

    if(sonarProxy->GetRange(4) > 0.2)
    {
      stop = false;
      p2dProxy->SetSpeed(-0.05,0);
    }

    while(!stop)
    {
      if(passos > Npassos )
      {
        p2dProxy->SetSpeed(0,0);
        break;
      }

      robot->ReadIfWaiting();
      if(this->BackBumper() != 0 or sonarProxy->GetRange(4) < BACK_RANGER or sonarProxy->GetRange(3) < SIDE_RANGER or sonarProxy->GetRange(5) < SIDE_RANGER)
      {
        p2dProxy->SetSpeed(0,0);
        stop = true;
        andou = hypotf(p2dProxy->GetXPos() - posxi, p2dProxy->GetYPos() - posyi);
        erro = andou - (int)andou;
        break;
      }

      robot->ReadIfWaiting();
      if(sonarProxy->GetRange(4) < 0.2 and !obstacle) // For donnie, do not work in stage.
      {
        Npassos = passos;
        obstacle = true;
      }

      robot->ReadIfWaiting();
      if(hypotf(p2dProxy->GetXPos() - posxi, p2dProxy->GetYPos() - posyi) > path[passos])
      {
            passos++;
      }

    path.clear();

    #ifndef NDEBUG
    cout << "Andou: " << passos << ", parou: " << stop << ", erro: " << erro << ", obstaculo: " << obstacle << endl;
    #endif

    // number of steps actually taken
    return passos;  
}

[richmattes]

The "GoTo" function is designed to turn towards the goal position and move towards it. It's implemented here: https://github.com/rtv/Stage/blob/master/libstage/model_position.cc#L405

If you want to do the same thing, but travel backwards towards the goal, you could modify the algorithm that GoTo uses to do so by reversing the yaw angle and wheel speed logic.

[rtv]

Goto behaviour is as-designed. Most robots are not designed to go backwards as they have forward-facing sensors to detect obstacles. As jpgr87 describes, it would be pretty easy to modify to go backwards.