33 double safety_dist_0ms,
34 double safety_dist_1ms,
36 int max_speed_narrow_opening,
37 int max_speed_wide_opening,
42 double min_turn_radius_safety_factor,
43 double free_space_cutoff_0ms,
44 double obs_cutoff_0ms,
45 double free_space_cutoff_1ms,
46 double obs_cutoff_1ms,
47 double weight_desired_dir,
48 double weight_current_dir );
61 int Update_VFH(
double laser_ranges[361][2],
65 float goal_distance_tolerance,
67 int &chosen_turnrate );
70 int GetMinTurnrate() {
return MIN_TURNRATE; }
72 float GetDesiredAngle() {
return Desired_Angle; }
73 float GetPickedAngle() {
return Picked_Angle; }
76 int GetMaxTurnrate(
int speed );
77 int GetCurrentMaxSpeed() {
return Current_Max_Speed; }
80 void SetRobotRadius(
float robot_radius ) { this->ROBOT_RADIUS = robot_radius; }
81 void SetMinTurnrate(
int min_turnrate ) { MIN_TURNRATE = min_turnrate; }
82 void SetCurrentMaxSpeed(
int Current_Max_Speed );
96 float Delta_Angle(
int a1,
int a2);
97 float Delta_Angle(
float a1,
float a2);
98 int Bisect_Angle(
int angle1,
int angle2);
100 bool Cant_Turn_To_Goal();
103 int Calculate_Cells_Mag(
double laser_ranges[361][2],
int speed );
105 int Build_Primary_Polar_Histogram(
double laser_ranges[361][2],
int speed );
106 int Build_Binary_Polar_Histogram(
int speed);
107 int Build_Masked_Polar_Histogram(
int speed);
108 int Select_Candidate_Angle();
109 int Select_Direction();
110 int Set_Motion(
int &speed,
int &turnrate,
int current_speed );
115 void Print_Cells_Dir();
116 void Print_Cells_Mag();
117 void Print_Cells_Dist();
118 void Print_Cells_Sector();
119 void Print_Cells_Enlargement_Angle();
124 int Get_Speed_Index(
int speed );
127 int Get_Safety_Dist(
int speed );
129 float Get_Binary_Hist_Low(
int speed );
130 float Get_Binary_Hist_High(
int speed );
142 float SAFETY_DIST_0MS;
143 float SAFETY_DIST_1MS;
144 int Current_Max_Speed;
146 int MAX_SPEED_NARROW_OPENING;
147 int MAX_SPEED_WIDE_OPENING;
148 int MAX_ACCELERATION;
151 int NUM_CELL_SECTOR_TABLES;
154 int MAX_TURNRATE_0MS;
155 int MAX_TURNRATE_1MS;
156 double MIN_TURN_RADIUS_SAFETY_FACTOR;
157 float Binary_Hist_Low_0ms, Binary_Hist_High_0ms;
158 float Binary_Hist_Low_1ms, Binary_Hist_High_1ms;
160 float Desired_Angle, Dist_To_Goal, Goal_Distance_Tolerance;
161 float Picked_Angle, Last_Picked_Angle;
162 int Max_Speed_For_Picked_Angle;
166 float Blocked_Circle_Radius;
168 std::vector<std::vector<float> > Cell_Direction;
169 std::vector<std::vector<float> > Cell_Base_Mag;
170 std::vector<std::vector<float> > Cell_Mag;
171 std::vector<std::vector<float> > Cell_Dist;
172 std::vector<std::vector<float> > Cell_Enlarge;
178 std::vector<std::vector<std::vector<std::vector<int> > > > Cell_Sector;
179 std::vector<float> Candidate_Angle;
180 std::vector<int> Candidate_Speed;
185 float *Last_Binary_Hist;
188 std::vector<int> Min_Turning_Radius;
191 timeval last_update_time;
193 int last_chosen_speed;