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