Home
FAQ
Player
Stage
Gazebo
Contrib
Documentation
Publications
Contributors
Users

Project
Download
Bugs/Feedback
Mailing lists

Radish

Old news
Old stuff

vfh_algorithm.h

Go to the documentation of this file.
00001 #ifndef VFH_ALGORITHM_H
00002 #define VFH_ALGORITHM_H
00003 
00004 #include <vector>
00005 #include "player.h"
00006 #include "playertime.h"
00007 
00008 class VFH_Algorithm
00009 {
00010 public:
00011     VFH_Algorithm( double cell_size,
00012                    int window_diameter,
00013                    int sector_angle,
00014                    double safety_dist_0ms,
00015                    double safety_dist_1ms, 
00016                    int max_speed,
00017                    int max_speed_narrow_opening,
00018                    int max_speed_wide_opening,
00019                    int max_acceleration,
00020                    int min_turnrate,
00021                    int max_turnrate_0ms,
00022                    int max_turnrate_1ms,
00023                    double min_turn_radius_safety_factor,
00024                    double free_space_cutoff_0ms,
00025                    double obs_cutoff_0ms,
00026                    double free_space_cutoff_1ms,
00027                    double obs_cutoff_1ms,
00028                    double weight_desired_dir,
00029                    double weight_current_dir );
00030 
00031     ~VFH_Algorithm();
00032 
00033     int Init();
00034     
00035     // Choose a new speed and turnrate based on the given laser data and current speed.
00036     int Update_VFH( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][2], int current_speed, int &chosen_speed, int &chosen_turnrate );
00037 
00038     // Get methods
00039     int   GetMinTurnrate() { return MIN_TURNRATE; }
00040     float GetDesiredAngle() { return Desired_Angle; }
00041     float GetPickedAngle() { return Picked_Angle; }
00042 
00043     // Max Turnrate depends on speed
00044     int GetMaxTurnrate( int speed );
00045     int GetCurrentMaxSpeed() { return Current_Max_Speed; }
00046 
00047     // Set methods
00048     void SetRobotRadius( float robot_radius ) { this->ROBOT_RADIUS = robot_radius; }
00049     void SetDesiredAngle( float Desired_Angle ) { this->Desired_Angle = Desired_Angle; }
00050     void SetMinTurnrate( int min_turnrate ) { MIN_TURNRATE = min_turnrate; }
00051     void SetCurrentMaxSpeed( int Current_Max_Speed );
00052 
00053     // The Histogram.
00054     // This is public so that monitoring tools can get at it; it shouldn't
00055     // be modified externally.
00056     // Sweeps in an anti-clockwise direction.
00057     float *Hist;
00058 
00059 private:
00060 
00061     // Functions
00062 
00063     int VFH_Allocate();
00064 
00065     float Delta_Angle(int a1, int a2);
00066     float Delta_Angle(float a1, float a2);
00067     int Bisect_Angle(int angle1, int angle2);
00068 
00069     // Returns 0 if something got inside the safety distance, else 1.
00070     int Calculate_Cells_Mag( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][2], int speed );
00071     // Returns 0 if something got inside the safety distance, else 1.
00072     int Build_Primary_Polar_Histogram( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][2], int speed );
00073     int Build_Binary_Polar_Histogram(int speed);
00074     int Build_Masked_Polar_Histogram(int speed);
00075     int Select_Candidate_Angle();
00076     int Select_Direction();
00077     int Set_Motion( int &speed, int &turnrate, int current_speed );
00078 
00079     // AB: This doesn't seem to be implemented anywhere...
00080     // int Read_Min_Turning_Radius_From_File(char *filename);
00081 
00082     void Print_Cells_Dir();
00083     void Print_Cells_Mag();
00084     void Print_Cells_Dist();
00085     void Print_Cells_Sector();
00086     void Print_Cells_Enlargement_Angle();
00087     void Print_Hist();
00088 
00089     // Returns the speed index into Cell_Sector, for a given speed in mm/sec.
00090     // This exists so that only a few (potentially large) Cell_Sector tables must be stored.
00091     int Get_Speed_Index( int speed );
00092 
00093     // Returns the safety dist in mm for this speed.
00094     int Get_Safety_Dist( int speed );
00095 
00096     float Get_Binary_Hist_Low( int speed );
00097     float Get_Binary_Hist_High( int speed );
00098 
00099     // Data
00100 
00101     float ROBOT_RADIUS;           // millimeters
00102     int CENTER_X;                 // cells
00103     int CENTER_Y;                 // cells
00104     int HIST_SIZE;                // sectors (over 360deg)
00105 
00106     float CELL_WIDTH;             // millimeters
00107     int WINDOW_DIAMETER;          // cells
00108     int SECTOR_ANGLE;             // degrees
00109     float SAFETY_DIST_0MS;        // millimeters
00110     float SAFETY_DIST_1MS;        // millimeters
00111     int Current_Max_Speed;        // mm/sec
00112     int MAX_SPEED;                // mm/sec
00113     int MAX_SPEED_NARROW_OPENING; // mm/sec
00114     int MAX_SPEED_WIDE_OPENING;   // mm/sec
00115     int MAX_ACCELERATION;         // mm/sec/sec
00116     int MIN_TURNRATE;             // deg/sec -- not actually used internally
00117 
00118     int NUM_CELL_SECTOR_TABLES;
00119 
00120     // Scale turnrate linearly between these two
00121     int MAX_TURNRATE_0MS;       // deg/sec
00122     int MAX_TURNRATE_1MS;       // deg/sec
00123     double MIN_TURN_RADIUS_SAFETY_FACTOR;
00124     float Binary_Hist_Low_0ms, Binary_Hist_High_0ms;
00125     float Binary_Hist_Low_1ms, Binary_Hist_High_1ms;
00126     float U1, U2;
00127     float Desired_Angle, Picked_Angle, Last_Picked_Angle;
00128     int   Max_Speed_For_Picked_Angle;
00129 
00130     std::vector<std::vector<float> > Cell_Direction;
00131     std::vector<std::vector<float> > Cell_Base_Mag;
00132     std::vector<std::vector<float> > Cell_Mag;
00133     std::vector<std::vector<float> > Cell_Dist;      // millimetres
00134     std::vector<std::vector<float> > Cell_Enlarge;
00135 
00136     // Cell_Sector[x][y] is a vector of indices to sectors that are effected if cell (x,y) contains
00137     // an obstacle.  
00138     // Cell enlargement is taken into account.
00139     // Acess as: Cell_Sector[speed_index][x][y][sector_index]
00140     std::vector<std::vector<std::vector<std::vector<int> > > > Cell_Sector;
00141     std::vector<float> Candidate_Angle;
00142     std::vector<int> Candidate_Speed;
00143 
00144     double dist_eps;
00145     double ang_eps;
00146 
00147     float *Last_Binary_Hist;
00148 
00149     // Minimum turning radius at different speeds, in millimeters
00150     std::vector<int> Min_Turning_Radius;
00151 
00152     // Keep track of last update, so we can monitor acceleration
00153     timeval last_update_time;
00154 
00155     int last_chosen_speed;
00156 };
00157 
00158 #endif

Generated on Tue May 3 14:15:37 2005 for Player by doxygen 1.3.6