vfh_algorithm.cc

机器人仿真软件

        new_angle = border[i].first + (border[i].second - border[i].first) / 2.0;

        Candidate_Angle.push_back(new_angle);
        Candidate_Speed.push_back(MIN(Current_Max_Speed,MAX_SPEED_NARROW_OPENING));
    } 
    else 
    {
        // wide opening: consider the centre, and 40deg from each border

        new_angle = border[i].first + (border[i].second - border[i].first) / 2.0;

        Candidate_Angle.push_back(new_angle);
        Candidate_Speed.push_back(Current_Max_Speed);

        new_angle = (float)((border[i].first + 40) % 360);
        Candidate_Angle.push_back(new_angle);
        Candidate_Speed.push_back(MIN(Current_Max_Speed,MAX_SPEED_WIDE_OPENING));

        new_angle = (float)(border[i].second - 40);
        if (new_angle < 0) 
            new_angle += 360;
        Candidate_Angle.push_back(new_angle);
        Candidate_Speed.push_back(MIN(Current_Max_Speed,MAX_SPEED_WIDE_OPENING));
        
        // See if candidate dir is in this opening
        if ((Delta_Angle(Desired_Angle, Candidate_Angle[Candidate_Angle.size()-2]) < 0) && 
            (Delta_Angle(Desired_Angle, Candidate_Angle[Candidate_Angle.size()-1]) > 0)) {
            Candidate_Angle.push_back(Desired_Angle);
            Candidate_Speed.push_back(MIN(Current_Max_Speed,MAX_SPEED_WIDE_OPENING));
        }
    }
  }

  Select_Candidate_Angle();

  return(1);
}

void VFH_Algorithm::Print_Cells_Dir() 
{
  int x, y;

  printf("\nCell Directions:\n");
  printf("****************\n");
  for(y=0;y<WINDOW_DIAMETER;y++) {
    for(x=0;x<WINDOW_DIAMETER;x++) {
      printf("%1.1f\t", Cell_Direction[x][y]);
    }
    printf("\n");
  }
}

void VFH_Algorithm::Print_Cells_Mag() 
{
  int x, y;

  printf("\nCell Magnitudes:\n");
  printf("****************\n");
  for(y=0;y<WINDOW_DIAMETER;y++) {
    for(x=0;x<WINDOW_DIAMETER;x++) {
      printf("%1.1f\t", Cell_Mag[x][y]);
    }
    printf("\n");
  }
}

void VFH_Algorithm::Print_Cells_Dist() 
{
  int x, y;

  printf("\nCell Distances:\n");
  printf("****************\n");
  for(y=0;y<WINDOW_DIAMETER;y++) {
    for(x=0;x<WINDOW_DIAMETER;x++) {
      printf("%1.1f\t", Cell_Dist[x][y]);
    }
    printf("\n");
  }
}

void VFH_Algorithm::Print_Cells_Sector() 
{
  int x, y;
  unsigned int i;

  printf("\nCell Sectors for table 0:\n");
  printf("***************************\n");

  for(y=0;y<WINDOW_DIAMETER;y++) {
    for(x=0;x<WINDOW_DIAMETER;x++) {
      for(i=0;i<Cell_Sector[0][x][y].size();i++) {
        if (i < (Cell_Sector[0][x][y].size() -1 )) {
          printf("%d,", Cell_Sector[0][x][y][i]);
        } else {
          printf("%d\t", Cell_Sector[0][x][y][i]);
        }
      }
    }
    printf("\n");
  }
}

void VFH_Algorithm::Print_Cells_Enlargement_Angle() 
{
  int x, y;

  printf("\nEnlargement Angles:\n");
  printf("****************\n");
  for(y=0;y<WINDOW_DIAMETER;y++) {
    for(x=0;x<WINDOW_DIAMETER;x++) {
      printf("%1.1f\t", Cell_Enlarge[x][y]);
    }
    printf("\n");
  }
}

void VFH_Algorithm::Print_Hist() 
{
  int x;
  printf("Histogram:\n");
  printf("****************\n");

  for(x=0;x<=(HIST_SIZE/2);x++) {
    printf("%d:\t%1.1f\n", (x * SECTOR_ANGLE), Hist[x]);
  }
  printf("\n\n");
}

int VFH_Algorithm::Calculate_Cells_Mag( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][2], int speed ) 
{
  int x, y;

/*
printf("Laser Ranges\n");
printf("************\n");
for(x=0;x<=360;x++) {
printf("%d: %f\n", x, this->laser_ranges[x][0]);
}
*/

  // AB: This is a bit dodgy...  Makes it possible to miss really skinny obstacles, since if the 
  //     resolution of the cells is finer than the resolution of laser_ranges, some ranges might be missed.
  //     Rather than looping over the cells, should perhaps loop over the laser_ranges.

  float r = ROBOT_RADIUS + Get_Safety_Dist(speed);

  // Only deal with the cells in front of the robot, since we can't sense behind.
  for(x=0;x<WINDOW_DIAMETER;x++) 
  {
      for(y=0;y<(int)ceil(WINDOW_DIAMETER/2.0);y++) 
      {
          if ((Cell_Dist[x][y] + CELL_WIDTH / 2.0) > 
              laser_ranges[(int)rint(Cell_Direction[x][y] * 2.0)][0]) 
          {
              if ( Cell_Dist[x][y] < r && !(x==CENTER_X && y==CENTER_Y) )
              {
                  // printf("Cell %d,%d: Cell_Dist is %f, range is %f (minimum is %f): too close...\n",
                  //        x,
                  //        y,
                  //        Cell_Dist[x][y] + CELL_WIDTH / 2.0,
                  //        laser_ranges[(int)rint(Cell_Direction[x][y] * 2.0)][0],
                  //        r);

                  // Damn, something got inside our safety_distance...
                  // Short-circuit this process.
                  return(0);
              }
              else
              {
                  Cell_Mag[x][y] = Cell_Base_Mag[x][y];
              }
          } else {
              Cell_Mag[x][y] = 0.0;
          }
      }
  }

  return(1);
}

int VFH_Algorithm::Build_Primary_Polar_Histogram( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][2], int speed ) 
{
  int x, y;
  unsigned int i;
  // index into the vector of Cell_Sector tables
  int speed_index = Get_Speed_Index( speed );

  for(x=0;x<HIST_SIZE;x++) {
    Hist[x] = 0;
  }

  if ( Calculate_Cells_Mag( laser_ranges, speed ) == 0 )
  {
      // set Hist to all blocked
      for(x=0;x<HIST_SIZE;x++) {
          Hist[x] = 1;
      }
      return 0;
  }

//  Print_Cells_Dist();
//  Print_Cells_Dir();
//  Print_Cells_Mag();
//  Print_Cells_Sector();
//  Print_Cells_Enlargement_Angle();

  // Only have to go through the cells in front.
  for(y=0;y<=(int)ceil(WINDOW_DIAMETER/2.0);y++) {
    for(x=0;x<WINDOW_DIAMETER;x++) {
      for(i=0;i<Cell_Sector[speed_index][x][y].size();i++) {
        Hist[Cell_Sector[speed_index][x][y][i]] += Cell_Mag[x][y];
      }
    }
  }

  return(1);
}

int VFH_Algorithm::Build_Binary_Polar_Histogram( int speed ) 
{
  int x;

  for(x=0;x<HIST_SIZE;x++) {
      if (Hist[x] > Get_Binary_Hist_High(speed)) {
      Hist[x] = 1.0;
      } else if (Hist[x] < Get_Binary_Hist_Low(speed)) {
      Hist[x] = 0.0;
    } else {
      Hist[x] = Last_Binary_Hist[x];
    }
  }

  for(x=0;x<HIST_SIZE;x++) {
    Last_Binary_Hist[x] = Hist[x];
  }

  return(1);
}

//
// This function also sets Blocked_Circle_Radius.
//
int VFH_Algorithm::Build_Masked_Polar_Histogram(int speed) 
{
  int x, y;
  float center_x_right, center_x_left, center_y, dist_r, dist_l;
  float angle_ahead, phi_left, phi_right, angle;

  // center_x_[left|right] is the centre of the circles on either side that
  // are blocked due to the robot's dynamics.  Units are in cells, in the robot's
  // local coordinate system (+y is forward).
  center_x_right = CENTER_X + (Min_Turning_Radius[speed] / (float)CELL_WIDTH);
  center_x_left = CENTER_X - (Min_Turning_Radius[speed] / (float)CELL_WIDTH);
  center_y = CENTER_Y;

  angle_ahead = 90;
  phi_left  = 180;
  phi_right = 0;

  Blocked_Circle_Radius = Min_Turning_Radius[speed] + ROBOT_RADIUS + Get_Safety_Dist(speed);

  //
  // This loop fixes phi_left and phi_right so that they go through the inside-most
  // occupied cells inside the left/right circles.  These circles are centred at the 
  // left/right centres of rotation, and are of radius Blocked_Circle_Radius.
  // 
  // We have to go between phi_left and phi_right, due to our minimum turning radius.
  //

  //
  // Only loop through the cells in front of us.
  //
  for(y=0;y<(int)ceil(WINDOW_DIAMETER/2.0);y++) 
  {
    for(x=0;x<WINDOW_DIAMETER;x++) 
    {
        if (Cell_Mag[x][y] == 0) 
            continue;

        if ((Delta_Angle(Cell_Direction[x][y], angle_ahead) > 0) && 
            (Delta_Angle(Cell_Direction[x][y], phi_right) <= 0)) 
        {
            // The cell is between phi_right and angle_ahead

            dist_r = hypot(center_x_right - x, center_y - y) * CELL_WIDTH;
            if (dist_r < Blocked_Circle_Radius) 
            { 
                phi_right = Cell_Direction[x][y];
            }
        } 
        else if ((Delta_Angle(Cell_Direction[x][y], angle_ahead) <= 0) && 
                 (Delta_Angle(Cell_Direction[x][y], phi_left) > 0)) 
        {
            // The cell is between phi_left and angle_ahead

            dist_l = hypot(center_x_left - x, center_y - y) * CELL_WIDTH;
            if (dist_l < Blocked_Circle_Radius) 
            { 
                phi_left = Cell_Direction[x][y];
            }
        }
    }
  }

  //
  // Mask out everything outside phi_left and phi_right
  //
  for(x=0;x<HIST_SIZE;x++) 
  {
      angle = x * SECTOR_ANGLE;
      if ((Hist[x] == 0) && (((Delta_Angle((float)angle, phi_right) <= 0) && 
                              (Delta_Angle((float)angle, angle_ahead) >= 0)) || 
                             ((Delta_Angle((float)angle, phi_left) >= 0) &&
                              (Delta_Angle((float)angle, angle_ahead) <= 0)))) 
      {
          Hist[x] = 0;
      } 
      else 
      {
          Hist[x] = 1;
      }
  }

  return(1);
}


int VFH_Algorithm::Set_Motion( int &speed, int &turnrate, int actual_speed ) 
{
  // This happens if all directions blocked, so just spin in place
  if (speed <= 0) 
  {
    //printf("stop\n");
      turnrate = GetMaxTurnrate( actual_speed );
      speed = 0;
  }
  else 
  {
    //printf("Picked %f\n", Picked_Angle);
    if ((Picked_Angle > 270) && (Picked_Angle < 360)) {
        turnrate = -1 * GetMaxTurnrate( actual_speed );
    } else if ((Picked_Angle < 270) && (Picked_Angle > 180)) {
      turnrate = GetMaxTurnrate( actual_speed );
    } else {
      turnrate = (int)rint(((float)(Picked_Angle - 90) / 75.0) * GetMaxTurnrate( actual_speed ));

      if (turnrate > GetMaxTurnrate( actual_speed )) {
        turnrate = GetMaxTurnrate( actual_speed );
      } else if (turnrate < (-1 * GetMaxTurnrate( actual_speed ))) {
        turnrate = -1 * GetMaxTurnrate( actual_speed );
      }

//      if (abs(turnrate) > (0.9 * GetMaxTurnrate( actual_speed ))) {
//        speed = 0;
//      }
    }
  }

//  speed and turnrate have been set for the calling function -- return.

  return(1);
}