vfh_algorithm.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00055
00056
00057
00058
00059
00060
00061 int Update_VFH( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][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
00070 int GetMinTurnrate() { return MIN_TURNRATE; }
00071
00072 float GetDesiredAngle() { return Desired_Angle; }
00073 float GetPickedAngle() { return Picked_Angle; }
00074
00075
00076 int GetMaxTurnrate( int speed );
00077 int GetCurrentMaxSpeed() { return Current_Max_Speed; }
00078
00079
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
00085
00086
00087
00088 float *Hist;
00089
00090 private:
00091
00092
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
00103 int Calculate_Cells_Mag( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][2], int speed );
00104
00105 int Build_Primary_Polar_Histogram( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][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
00113
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
00123
00124 int Get_Speed_Index( int speed );
00125
00126
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
00133
00134 float ROBOT_RADIUS;
00135 int CENTER_X;
00136 int CENTER_Y;
00137 int HIST_SIZE;
00138
00139 float CELL_WIDTH;
00140 int WINDOW_DIAMETER;
00141 int SECTOR_ANGLE;
00142 float SAFETY_DIST_0MS;
00143 float SAFETY_DIST_1MS;
00144 int Current_Max_Speed;
00145 int MAX_SPEED;
00146 int MAX_SPEED_NARROW_OPENING;
00147 int MAX_SPEED_WIDE_OPENING;
00148 int MAX_ACCELERATION;
00149 int MIN_TURNRATE;
00150
00151 int NUM_CELL_SECTOR_TABLES;
00152
00153
00154 int MAX_TURNRATE_0MS;
00155 int MAX_TURNRATE_1MS;
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
00165
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;
00172 std::vector<std::vector<float> > Cell_Enlarge;
00173
00174
00175
00176
00177
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
00188 std::vector<int> Min_Turning_Radius;
00189
00190
00191 timeval last_update_time;
00192
00193 int last_chosen_speed;
00194 };
00195
00196 #endif
Last updated 12 September 2005 21:38:45
|