vfh_algorithm.h
00001 /*
00002  *  Orca-Components: Components for robotics.
00003  *  
00004  *  Copyright (C) 2004
00005  *  
00006  *  This program is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU General Public License
00008  *  as published by the Free Software Foundation; either version 2
00009  *  of the License, or (at your option) any later version.
00010  *  
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *  
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
00019  */
00020 
00021 #ifndef VFH_ALGORITHM_H
00022 #define VFH_ALGORITHM_H
00023 
00024 #include <vector>
00025 #include <libplayercore/playercore.h>
00026 
00027 class VFH_Algorithm
00028 {
00029 public:
00030     VFH_Algorithm( double cell_size,
00031                    int window_diameter,
00032                    int sector_angle,
00033                    double safety_dist_0ms,
00034                    double safety_dist_1ms, 
00035                    int max_speed,
00036                    int max_speed_narrow_opening,
00037                    int max_speed_wide_opening,
00038                    int max_acceleration,
00039                    int min_turnrate,
00040                    int max_turnrate_0ms,
00041                    int max_turnrate_1ms,
00042                    double min_turn_radius_safety_factor,
00043                    double free_space_cutoff_0ms,
00044                    double obs_cutoff_0ms,
00045                    double free_space_cutoff_1ms,
00046                    double obs_cutoff_1ms,
00047                    double weight_desired_dir,
00048                    double weight_current_dir );
00049 
00050     ~VFH_Algorithm();
00051 
00052     int Init();
00053     
00054     // Choose a new speed and turnrate based on the given laser data and current speed.
00055     //
00056     // Units/Senses:
00057     //  - goal_direction in degrees, 0deg is to the right.
00058     //  - goal_distance  in mm.
00059     //  - goal_distance_tolerance in mm.
00060     //
00061     int Update_VFH( double laser_ranges[361][2], 
00062                     int current_speed,  
00063                     float goal_direction,
00064                     float goal_distance,
00065                     float goal_distance_tolerance,
00066                     int &chosen_speed, 
00067                     int &chosen_turnrate );
00068 
00069     // Get methods
00070     int   GetMinTurnrate() { return MIN_TURNRATE; }
00071     // Angle to goal, in degrees.  0deg is to our right.
00072     float GetDesiredAngle() { return Desired_Angle; }
00073     float GetPickedAngle() { return Picked_Angle; }
00074 
00075     // Max Turnrate depends on speed
00076     int GetMaxTurnrate( int speed );
00077     int GetCurrentMaxSpeed() { return Current_Max_Speed; }
00078 
00079     // Set methods
00080     void SetRobotRadius( float robot_radius ) { this->ROBOT_RADIUS = robot_radius; }
00081     void SetMinTurnrate( int min_turnrate ) { MIN_TURNRATE = min_turnrate; }
00082     void SetCurrentMaxSpeed( int Current_Max_Speed );
00083 
00084     // The Histogram.
00085     // This is public so that monitoring tools can get at it; it shouldn't
00086     // be modified externally.
00087     // Sweeps in an anti-clockwise direction.
00088     float *Hist;
00089 
00090 private:
00091 
00092     // Functions
00093 
00094     int VFH_Allocate();
00095 
00096     float Delta_Angle(int a1, int a2);
00097     float Delta_Angle(float a1, float a2);
00098     int Bisect_Angle(int angle1, int angle2);
00099 
00100     bool Cant_Turn_To_Goal();
00101 
00102     // Returns 0 if something got inside the safety distance, else 1.
00103     int Calculate_Cells_Mag( double laser_ranges[361][2], int speed );
00104     // Returns 0 if something got inside the safety distance, else 1.
00105     int Build_Primary_Polar_Histogram( double laser_ranges[361][2], int speed );
00106     int Build_Binary_Polar_Histogram(int speed);
00107     int Build_Masked_Polar_Histogram(int speed);
00108     int Select_Candidate_Angle();
00109     int Select_Direction();
00110     int Set_Motion( int &speed, int &turnrate, int current_speed );
00111 
00112     // AB: This doesn't seem to be implemented anywhere...
00113     // int Read_Min_Turning_Radius_From_File(char *filename);
00114 
00115     void Print_Cells_Dir();
00116     void Print_Cells_Mag();
00117     void Print_Cells_Dist();
00118     void Print_Cells_Sector();
00119     void Print_Cells_Enlargement_Angle();
00120     void Print_Hist();
00121 
00122     // Returns the speed index into Cell_Sector, for a given speed in mm/sec.
00123     // This exists so that only a few (potentially large) Cell_Sector tables must be stored.
00124     int Get_Speed_Index( int speed );
00125 
00126     // Returns the safety dist in mm for this speed.
00127     int Get_Safety_Dist( int speed );
00128 
00129     float Get_Binary_Hist_Low( int speed );
00130     float Get_Binary_Hist_High( int speed );
00131 
00132     // Data
00133 
00134     float ROBOT_RADIUS;           // millimeters
00135     int CENTER_X;                 // cells
00136     int CENTER_Y;                 // cells
00137     int HIST_SIZE;                // sectors (over 360deg)
00138 
00139     float CELL_WIDTH;             // millimeters
00140     int WINDOW_DIAMETER;          // cells
00141     int SECTOR_ANGLE;             // degrees
00142     float SAFETY_DIST_0MS;        // millimeters
00143     float SAFETY_DIST_1MS;        // millimeters
00144     int Current_Max_Speed;        // mm/sec
00145     int MAX_SPEED;                // mm/sec
00146     int MAX_SPEED_NARROW_OPENING; // mm/sec
00147     int MAX_SPEED_WIDE_OPENING;   // mm/sec
00148     int MAX_ACCELERATION;         // mm/sec/sec
00149     int MIN_TURNRATE;             // deg/sec -- not actually used internally
00150 
00151     int NUM_CELL_SECTOR_TABLES;
00152 
00153     // Scale turnrate linearly between these two
00154     int MAX_TURNRATE_0MS;       // deg/sec
00155     int MAX_TURNRATE_1MS;       // deg/sec
00156     double MIN_TURN_RADIUS_SAFETY_FACTOR;
00157     float Binary_Hist_Low_0ms, Binary_Hist_High_0ms;
00158     float Binary_Hist_Low_1ms, Binary_Hist_High_1ms;
00159     float U1, U2;
00160     float Desired_Angle, Dist_To_Goal, Goal_Distance_Tolerance;
00161     float Picked_Angle, Last_Picked_Angle;
00162     int   Max_Speed_For_Picked_Angle;
00163 
00164     // Radius of dis-allowed circles, either side of the robot, which
00165     // we can't enter due to our minimum turning radius.
00166     float Blocked_Circle_Radius;
00167 
00168     std::vector<std::vector<float> > Cell_Direction;
00169     std::vector<std::vector<float> > Cell_Base_Mag;
00170     std::vector<std::vector<float> > Cell_Mag;
00171     std::vector<std::vector<float> > Cell_Dist;      // millimetres
00172     std::vector<std::vector<float> > Cell_Enlarge;
00173 
00174     // Cell_Sector[x][y] is a vector of indices to sectors that are effected if cell (x,y) contains
00175     // an obstacle.  
00176     // Cell enlargement is taken into account.
00177     // Acess as: Cell_Sector[speed_index][x][y][sector_index]
00178     std::vector<std::vector<std::vector<std::vector<int> > > > Cell_Sector;
00179     std::vector<float> Candidate_Angle;
00180     std::vector<int> Candidate_Speed;
00181 
00182     double dist_eps;
00183     double ang_eps;
00184 
00185     float *Last_Binary_Hist;
00186 
00187     // Minimum turning radius at different speeds, in millimeters
00188     std::vector<int> Min_Turning_Radius;
00189 
00190     // Keep track of last update, so we can monitor acceleration
00191     timeval last_update_time;
00192 
00193     int last_chosen_speed;
00194 };
00195 
00196 #endif

Last updated 12 September 2005 21:38:45