AlgoVFH.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00048
00049
00050
00051 #ifndef __VFH_ALGO_H
00052 #define __VFH_ALGO_H
00053
00054 #include <baselib.h>
00055
00056 #include <vector>
00057 #include <math.h>
00058
00059
00060
00061
00065 class VFH_Algorithm
00066 {
00067 public:
00087 VFH_Algorithm( Decimal cell_size,
00088 int window_diameter,
00089 int sector_angle,
00090 Decimal safety_dist_0ms,
00091 Decimal safety_dist_1ms,
00092 int max_speed,
00093 int max_speed_narrow_opening,
00094 int max_speed_wide_opening,
00095 int max_acceleration,
00096 int min_turnrate,
00097 int max_turnrate_0ms,
00098 int max_turnrate_1ms,
00099 Decimal min_turn_radius_safety_factor,
00100 Decimal free_space_cutoff_0ms,
00101 Decimal obs_cutoff_0ms,
00102 Decimal free_space_cutoff_1ms,
00103 Decimal obs_cutoff_1ms,
00104 Decimal weight_desired_dir,
00105 Decimal weight_current_dir );
00106
00107 ~VFH_Algorithm();
00108
00109 int Init();
00110
00119 int Update_VFH( Decimal laser_ranges[361][2],
00120 int current_speed,
00121 Decimal goal_direction,
00122 Decimal goal_distance,
00123 Decimal goal_distance_tolerance,
00124 int &chosen_speed,
00125 int &chosen_turnrate );
00126
00127
00130 int GetMinTurnrate() { return MIN_TURNRATE; }
00131
00132
00135 Decimal GetDesiredAngle() { return Desired_Angle; }
00136
00139 Decimal GetPickedAngle() { return Picked_Angle; }
00140
00141
00145 int GetMaxTurnrate( int speed );
00146
00149 int GetCurrentMaxSpeed() { return Current_Max_Speed; }
00150
00151
00155 void SetRobotRadius( Decimal robot_radius ) { this->ROBOT_RADIUS = robot_radius; }
00156
00160 void SetMinTurnrate( int min_turnrate ) { MIN_TURNRATE = min_turnrate; }
00161
00165 void SetCurrentMaxSpeed( int Current_Max_Speed );
00166
00167
00168
00169
00170
00171 Decimal *Hist;
00172
00173 private:
00174
00175
00178 int VFH_Allocate();
00179
00184 Decimal Delta_Angle(int a1, int a2);
00185
00190 Decimal Delta_Angle(Decimal a1, Decimal a2);
00191
00196 int Bisect_Angle(int angle1, int angle2);
00197
00200 bool Cant_Turn_To_Goal();
00201
00206 int Calculate_Cells_Mag( Decimal laser_ranges[361][2], int speed );
00207
00212 int Build_Primary_Polar_Histogram( Decimal laser_ranges[361][2], int speed );
00213
00217 int Build_Binary_Polar_Histogram(int speed);
00218
00222 int Build_Masked_Polar_Histogram(int speed);
00223
00226 int Select_Candidate_Angle();
00227
00230 int Select_Direction();
00231
00238 int Set_Motion( int &speed, int &turnrate, int current_speed, int current_turnrate);
00239
00240
00241
00244 void Print_Cells_Dir();
00245
00248 void Print_Cells_Mag();
00249
00252 void Print_Cells_Dist();
00253
00256 void Print_Cells_Sector();
00257
00260 void Print_Cells_Enlargement_Angle();
00261
00264 void Print_Hist();
00265
00266
00267
00271 int Get_Speed_Index( int speed );
00272
00273
00277 int Get_Safety_Dist( int speed );
00278
00282 Decimal Get_Binary_Hist_Low( int speed );
00283
00287 Decimal Get_Binary_Hist_High( int speed );
00288
00289
00290
00291 Decimal ROBOT_RADIUS;
00292 int CENTER_X;
00293 int CENTER_Y;
00294 int HIST_SIZE;
00295
00296 Decimal CELL_WIDTH;
00297 int WINDOW_DIAMETER;
00298 int SECTOR_ANGLE;
00299 Decimal SAFETY_DIST_0MS;
00300 Decimal SAFETY_DIST_1MS;
00301 int Current_Max_Speed;
00302 int MAX_SPEED;
00303 int MAX_SPEED_NARROW_OPENING;
00304 int MAX_SPEED_WIDE_OPENING;
00305 int MAX_ACCELERATION;
00306 int MIN_TURNRATE;
00307
00308 int NUM_CELL_SECTOR_TABLES;
00309
00310
00311 int MAX_TURNRATE_0MS;
00312 int MAX_TURNRATE_1MS;
00313 Decimal MIN_TURN_RADIUS_SAFETY_FACTOR;
00314 Decimal Binary_Hist_Low_0ms, Binary_Hist_High_0ms;
00315 Decimal Binary_Hist_Low_1ms, Binary_Hist_High_1ms;
00316 Decimal U1, U2;
00317 Decimal Desired_Angle, Dist_To_Goal, Goal_Distance_Tolerance;
00318 Decimal Picked_Angle, Last_Picked_Angle;
00319 int Max_Speed_For_Picked_Angle;
00320
00321
00322
00323 Decimal Blocked_Circle_Radius;
00324
00325 std::vector<std::vector<Decimal> > Cell_Direction;
00326 std::vector<std::vector<Decimal> > Cell_Base_Mag;
00327 std::vector<std::vector<Decimal> > Cell_Mag;
00328 std::vector<std::vector<Decimal> > Cell_Dist;
00329 std::vector<std::vector<Decimal> > Cell_Enlarge;
00330
00331
00332
00333
00334
00335 std::vector<std::vector<std::vector<std::vector<int> > > > Cell_Sector;
00336 std::vector<Decimal> Candidate_Angle;
00337 std::vector<int> Candidate_Speed;
00338
00339 Decimal dist_eps;
00340 Decimal ang_eps;
00341
00342 Decimal *Last_Binary_Hist;
00343
00344
00345 std::vector<int> Min_Turning_Radius;
00346
00347
00348 timeval last_update_time;
00349
00350 int last_chosen_speed;
00351 int last_chosen_turnrate;
00352 };
00353
00354 #endif
00355
00356