vfh_algorithm.cc

机器人仿真软件

/*
 *  Orca-Components: Components for robotics.
 *  
 *  Copyright (C) 2004
 *  
 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or (at your option) any later version.
 *  
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *  
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#include "vfh_algorithm.h"

#include <stdio.h>
#include <assert.h>
#include <math.h>

#include <libplayercore/playercore.h>

extern PlayerTime *GlobalTime;

VFH_Algorithm::VFH_Algorithm( double cell_size,
                              int window_diameter,
                              int sector_angle,
                              double safety_dist_0ms,
                              double safety_dist_1ms,
                              int max_speed,
                              int max_speed_narrow_opening,
                              int max_speed_wide_opening,
                              int max_acceleration,
                              int min_turnrate,
                              int max_turnrate_0ms,
                              int max_turnrate_1ms,
                              double min_turn_radius_safety_factor,
                              double free_space_cutoff_0ms,
                              double obs_cutoff_0ms,
                              double free_space_cutoff_1ms,
                              double obs_cutoff_1ms,
                              double weight_desired_dir,
                              double weight_current_dir )
    : CELL_WIDTH(cell_size),
      WINDOW_DIAMETER(window_diameter),
      SECTOR_ANGLE(sector_angle),
      SAFETY_DIST_0MS(safety_dist_0ms),
      SAFETY_DIST_1MS(safety_dist_1ms),
      Current_Max_Speed(max_speed),
      MAX_SPEED(max_speed),
      MAX_SPEED_NARROW_OPENING(max_speed_narrow_opening),
      MAX_SPEED_WIDE_OPENING(max_speed_wide_opening),
      MAX_ACCELERATION(max_acceleration),
      MIN_TURNRATE(min_turnrate),
      MAX_TURNRATE_0MS(max_turnrate_0ms),
      MAX_TURNRATE_1MS(max_turnrate_1ms),
      MIN_TURN_RADIUS_SAFETY_FACTOR(min_turn_radius_safety_factor),
      Binary_Hist_Low_0ms(free_space_cutoff_0ms),
      Binary_Hist_High_0ms(obs_cutoff_0ms),
      Binary_Hist_Low_1ms(free_space_cutoff_1ms),
      Binary_Hist_High_1ms(obs_cutoff_1ms),
      U1(weight_desired_dir),
      U2(weight_current_dir),
      Desired_Angle(90),
      Picked_Angle(90),
      Last_Picked_Angle(Picked_Angle),
      last_chosen_speed(0)
{
    this->Last_Binary_Hist = NULL;
    this->Hist = NULL;
    if ( SAFETY_DIST_0MS == SAFETY_DIST_1MS )
    {
        // For the simple case of a fixed safety_dist, keep things simple.
        NUM_CELL_SECTOR_TABLES = 1;  
    }
    else
    {
        // AB: Made this number up...
        NUM_CELL_SECTOR_TABLES = 20;
    }
}

VFH_Algorithm::~VFH_Algorithm()
{
    if(this->Hist)
        delete[] Hist;
    if(this->Last_Binary_Hist)
        delete[] Last_Binary_Hist;
}

int 
VFH_Algorithm::GetMaxTurnrate( int speed )
{ 
    int val = ( MAX_TURNRATE_0MS - (int)(speed*( MAX_TURNRATE_0MS-MAX_TURNRATE_1MS )/1000.0) );

    if ( val < 0 )
        val = 0;

    return val;
}

void
VFH_Algorithm::SetCurrentMaxSpeed( int max_speed )
{
    this->Current_Max_Speed = MIN( max_speed, this->MAX_SPEED );
    this->Min_Turning_Radius.resize( Current_Max_Speed+1 );

    // small chunks of forward movements and turns-in-place used to
    // estimate turning radius, coz I'm too lazy to screw around with limits -> 0.
    double dx, dtheta;

    //
    // Calculate the turning radius, indexed by speed.
    // Probably don't need it to be precise (changing in 1mm increments).
    //
    // WARNING: This assumes that the max_turnrate that has been set for VFH is
    //          accurate.
    //
    for(int x=0;x<=Current_Max_Speed;x++) 
    {
        dx = (double) x / 1e6; // dx in m/millisec
        dtheta = ((M_PI/180)*(double)(GetMaxTurnrate(x))) / 1000.0; // dTheta in radians/millisec
        Min_Turning_Radius[x] = (int) ( ((dx / tan( dtheta ))*1000.0) * MIN_TURN_RADIUS_SAFETY_FACTOR ); // in mm
    }
}


// Doesn't need optimization: only gets called once per update.
int  
VFH_Algorithm::Get_Speed_Index( int speed )
{
    int val = (int) floor(((float)speed/(float)Current_Max_Speed)*NUM_CELL_SECTOR_TABLES);

    if ( val >= NUM_CELL_SECTOR_TABLES )
        val = NUM_CELL_SECTOR_TABLES-1;

    // printf("Speed_Index at %dmm/s: %d\n",speed,val);

    return val;
}

// Doesn't need optimization: only gets called on init plus once per update.
int  
VFH_Algorithm::Get_Safety_Dist( int speed )
{
    int val = (int) ( SAFETY_DIST_0MS + (int)(speed*( SAFETY_DIST_1MS-SAFETY_DIST_0MS )/1000.0) );

    if ( val < 0 )
        val = 0;

    // printf("Safety_Dist at %dmm/s: %d\n",speed,val);

    return val;
}

// AB: Could optimize this with a look-up table, but it shouldn't make much 
//     difference: only gets called once per sector per update.
float
VFH_Algorithm::Get_Binary_Hist_Low( int speed )
{
    return ( Binary_Hist_Low_0ms - (speed*( Binary_Hist_Low_0ms-Binary_Hist_Low_1ms )/1000.0) );
}

// AB: Could optimize this with a look-up table, but it shouldn't make much 
//     difference: only gets called once per sector per update.
float
VFH_Algorithm::Get_Binary_Hist_High( int speed )
{
    return ( Binary_Hist_High_0ms - (speed*( Binary_Hist_High_0ms-Binary_Hist_High_1ms )/1000.0) );
}


int VFH_Algorithm::Init()
{
  int x, y, i;
  float plus_dir, neg_dir, plus_sector, neg_sector;
  bool plus_dir_bw, neg_dir_bw, dir_around_sector;
  float neg_sector_to_neg_dir, neg_sector_to_plus_dir;
  float plus_sector_to_neg_dir, plus_sector_to_plus_dir;
  int cell_sector_tablenum, max_speed_this_table;
  float r;

  CENTER_X = (int)floor(WINDOW_DIAMETER / 2.0);
  CENTER_Y = CENTER_X;
  HIST_SIZE = (int)rint(360.0 / SECTOR_ANGLE);

  // it works now; let's leave the verbose debug statement out
  /*
  printf("CELL_WIDTH: %1.1f\tWINDOW_DIAMETER: %d\tSECTOR_ANGLE: %d\tROBOT_RADIUS: %1.1f\tSAFETY_DIST: %1.1f\tMAX_SPEED: %d\tMAX_TURNRATE: %d\tFree Space Cutoff: %1.1f\tObs Cutoff: %1.1f\tWeight Desired Dir: %1.1f\tWeight Current_Dir:%1.1f\n", CELL_WIDTH, WINDOW_DIAMETER, SECTOR_ANGLE, ROBOT_RADIUS, SAFETY_DIST, MAX_SPEED, MAX_TURNRATE, Binary_Hist_Low, Binary_Hist_High, U1, U2);
  */

  VFH_Allocate();

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

  // For the following calcs: 
  //   - (x,y) = (0,0)   is to the front-left of the robot
  //   - (x,y) = (max,0) is to the front-right of the robot
  //
  for(x=0;x<WINDOW_DIAMETER;x++) {
    for(y=0;y<WINDOW_DIAMETER;y++) {
      Cell_Mag[x][y] = 0;
      Cell_Dist[x][y] = sqrt(pow((CENTER_X - x), 2) + pow((CENTER_Y - y), 2)) * CELL_WIDTH;

      Cell_Base_Mag[x][y] = pow((3000.0 - Cell_Dist[x][y]), 4) / 100000000.0;

      // Set up Cell_Direction with the angle in degrees to each cell
      if (x < CENTER_X) {
        if (y < CENTER_Y) {
          Cell_Direction[x][y] = atan((float)(CENTER_Y - y) / (float)(CENTER_X - x));
          Cell_Direction[x][y] *= (360.0 / 6.28);
          Cell_Direction[x][y] = 180.0 - Cell_Direction[x][y];
        } else if (y == CENTER_Y) {
          Cell_Direction[x][y] = 180.0;
        } else if (y > CENTER_Y) {
          Cell_Direction[x][y] = atan((float)(y - CENTER_Y) / (float)(CENTER_X - x));
          Cell_Direction[x][y] *= (360.0 / 6.28);
          Cell_Direction[x][y] = 180.0 + Cell_Direction[x][y];
        }
      } else if (x == CENTER_X) {
        if (y < CENTER_Y) {
          Cell_Direction[x][y] = 90.0;
        } else if (y == CENTER_Y) {
          Cell_Direction[x][y] = -1.0;
        } else if (y > CENTER_Y) {
          Cell_Direction[x][y] = 270.0;
        }
      } else if (x > CENTER_X) {
        if (y < CENTER_Y) {
          Cell_Direction[x][y] = atan((float)(CENTER_Y - y) / (float)(x - CENTER_X));
          Cell_Direction[x][y] *= (360.0 / 6.28);
        } else if (y == CENTER_Y) {
          Cell_Direction[x][y] = 0.0;
        } else if (y > CENTER_Y) {
          Cell_Direction[x][y] = atan((float)(y - CENTER_Y) / (float)(x - CENTER_X));
          Cell_Direction[x][y] *= (360.0 / 6.28);
          Cell_Direction[x][y] = 360.0 - Cell_Direction[x][y];
        }
      }

      // For the case where we have a speed-dependent safety_dist, calculate all tables
      for ( cell_sector_tablenum = 0; 
            cell_sector_tablenum < NUM_CELL_SECTOR_TABLES; 
            cell_sector_tablenum++ )
      {
        max_speed_this_table = (int) (((float)(cell_sector_tablenum+1)/(float)NUM_CELL_SECTOR_TABLES) * 
                                      (float) MAX_SPEED);

        // printf("cell_sector_tablenum: %d, max_speed: %d, safety_dist: %d\n",
        // cell_sector_tablenum,max_speed_this_table,Get_Safety_Dist(max_speed_this_table));

        // Set Cell_Enlarge to the _angle_ by which a an obstacle must be 
        // enlarged for this cell, at this speed
        if (Cell_Dist[x][y] > 0)
        {
          r = ROBOT_RADIUS + Get_Safety_Dist(max_speed_this_table);
          // Cell_Enlarge[x][y] = (float)atan( r / Cell_Dist[x][y] ) * (180/M_PI);
          Cell_Enlarge[x][y] = (float)asin( r / Cell_Dist[x][y] ) * (180/M_PI);
        }
        else
        {
          Cell_Enlarge[x][y] = 0;
        }

        Cell_Sector[cell_sector_tablenum][x][y].clear();
        plus_dir = Cell_Direction[x][y] + Cell_Enlarge[x][y];
        neg_dir  = Cell_Direction[x][y] - Cell_Enlarge[x][y];

        for(i=0;i<(360 / SECTOR_ANGLE);i++) 
        {
            // Set plus_sector and neg_sector to the angles to the two adjacent sectors
            plus_sector = (i + 1) * (float)SECTOR_ANGLE;
            neg_sector = i * (float)SECTOR_ANGLE;

            if ((neg_sector - neg_dir) > 180) {
                neg_sector_to_neg_dir = neg_dir - (neg_sector - 360);
            } else {
                if ((neg_dir - neg_sector) > 180) {
                    neg_sector_to_neg_dir = neg_sector - (neg_dir + 360);
                } else {
                    neg_sector_to_neg_dir = neg_dir - neg_sector;
                }
            }

            if ((plus_sector - neg_dir) > 180) {
                plus_sector_to_neg_dir = neg_dir - (plus_sector - 360);
            } else {
                if ((neg_dir - plus_sector) > 180) {
                    plus_sector_to_neg_dir = plus_sector - (neg_dir + 360);
                } else {
                    plus_sector_to_neg_dir = neg_dir - plus_sector;
                }
            }

            if ((plus_sector - plus_dir) > 180) {
                plus_sector_to_plus_dir = plus_dir - (plus_sector - 360);
            } else {
                if ((plus_dir - plus_sector) > 180) {
                    plus_sector_to_plus_dir = plus_sector - (plus_dir + 360);
                } else {
                    plus_sector_to_plus_dir = plus_dir - plus_sector;
                }
            }

            if ((neg_sector - plus_dir) > 180) {
                neg_sector_to_plus_dir = plus_dir - (neg_sector - 360);
            } else {
                if ((plus_dir - neg_sector) > 180) {
                    neg_sector_to_plus_dir = neg_sector - (plus_dir + 360);
                } else {
                    neg_sector_to_plus_dir = plus_dir - neg_sector;
                }
            }

            plus_dir_bw = 0;
            neg_dir_bw = 0;
            dir_around_sector = 0;

            if ((neg_sector_to_neg_dir >= 0) && (plus_sector_to_neg_dir <= 0)) {
                neg_dir_bw = 1; 
            }

            if ((neg_sector_to_plus_dir >= 0) && (plus_sector_to_plus_dir <= 0)) {
                plus_dir_bw = 1; 
            }

            if ((neg_sector_to_neg_dir <= 0) && (neg_sector_to_plus_dir >= 0)) {
                dir_around_sector = 1; 
            }

            if ((plus_sector_to_neg_dir <= 0) && (plus_sector_to_plus_dir >= 0)) {
                plus_dir_bw = 1; 
            }

            if ((plus_dir_bw) || (neg_dir_bw) || (dir_around_sector)) {
                Cell_Sector[cell_sector_tablenum][x][y].push_back(i);
            }
        }
      }
    }
  }

  assert( GlobalTime->GetTime( &last_update_time ) == 0 );

  // Print_Cells_Sector();

  return(1);
}

int VFH_Algorithm::VFH_Allocate() 
{
  std::vector<float> temp_vec;
  std::vector<int> temp_vec3;
  std::vector<std::vector<int> > temp_vec2;
  std::vector<std::vector<std::vector<int> > > temp_vec4;
  int x;