vfh_algorithm.h
1/*
2 * Orca-Components: Components for robotics.
3 *
4 * Copyright (C) 2004
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
19 */
20
21#ifndef VFH_ALGORITHM_H
22#define VFH_ALGORITHM_H
23
24#include <vector>
25#include <libplayercore/playercore.h>
26
28{
29public:
30 VFH_Algorithm( double cell_size,
31 int window_diameter,
32 int sector_angle,
33 double safety_dist_0ms,
34 double safety_dist_1ms,
35 int max_speed,
36 int max_speed_narrow_opening,
37 int max_speed_wide_opening,
38 int max_acceleration,
39 int min_turnrate,
40 int max_turnrate_0ms,
41 int max_turnrate_1ms,
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 );
49
51
52 int Init();
53
54 // Choose a new speed and turnrate based on the given laser data and current speed.
55 //
56 // Units/Senses:
57 // - goal_direction in degrees, 0deg is to the right.
58 // - goal_distance in mm.
59 // - goal_distance_tolerance in mm.
60 //
61 int Update_VFH( double laser_ranges[361][2],
62 int current_speed,
63 float goal_direction,
64 float goal_distance,
65 float goal_distance_tolerance,
66 int &chosen_speed,
67 int &chosen_turnrate );
68
69 // Get methods
70 int GetMinTurnrate() { return MIN_TURNRATE; }
71 // Angle to goal, in degrees. 0deg is to our right.
72 float GetDesiredAngle() { return Desired_Angle; }
73 float GetPickedAngle() { return Picked_Angle; }
74
75 // Max Turnrate depends on speed
76 int GetMaxTurnrate( int speed );
77 int GetCurrentMaxSpeed() { return Current_Max_Speed; }
78
79 // Set methods
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 );
83
84 // The Histogram.
85 // This is public so that monitoring tools can get at it; it shouldn't
86 // be modified externally.
87 // Sweeps in an anti-clockwise direction.
88 float *Hist;
89
90private:
91
92 // Functions
93
94 int VFH_Allocate();
95
96 float Delta_Angle(int a1, int a2);
97 float Delta_Angle(float a1, float a2);
98 int Bisect_Angle(int angle1, int angle2);
99
100 bool Cant_Turn_To_Goal();
101
102 // Returns 0 if something got inside the safety distance, else 1.
103 int Calculate_Cells_Mag( double laser_ranges[361][2], int speed );
104 // Returns 0 if something got inside the safety distance, else 1.
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 );
111
112 // AB: This doesn't seem to be implemented anywhere...
113 // int Read_Min_Turning_Radius_From_File(char *filename);
114
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();
120 void Print_Hist();
121
122 // Returns the speed index into Cell_Sector, for a given speed in mm/sec.
123 // This exists so that only a few (potentially large) Cell_Sector tables must be stored.
124 int Get_Speed_Index( int speed );
125
126 // Returns the safety dist in mm for this speed.
127 int Get_Safety_Dist( int speed );
128
129 float Get_Binary_Hist_Low( int speed );
130 float Get_Binary_Hist_High( int speed );
131
132 // Data
133
134 float ROBOT_RADIUS; // millimeters
135 int CENTER_X; // cells
136 int CENTER_Y; // cells
137 int HIST_SIZE; // sectors (over 360deg)
138
139 float CELL_WIDTH; // millimeters
140 int WINDOW_DIAMETER; // cells
141 int SECTOR_ANGLE; // degrees
142 float SAFETY_DIST_0MS; // millimeters
143 float SAFETY_DIST_1MS; // millimeters
144 int Current_Max_Speed; // mm/sec
145 int MAX_SPEED; // mm/sec
146 int MAX_SPEED_NARROW_OPENING; // mm/sec
147 int MAX_SPEED_WIDE_OPENING; // mm/sec
148 int MAX_ACCELERATION; // mm/sec/sec
149 int MIN_TURNRATE; // deg/sec -- not actually used internally
150
151 int NUM_CELL_SECTOR_TABLES;
152
153 // Scale turnrate linearly between these two
154 int MAX_TURNRATE_0MS; // deg/sec
155 int MAX_TURNRATE_1MS; // deg/sec
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;
159 float U1, U2;
160 float Desired_Angle, Dist_To_Goal, Goal_Distance_Tolerance;
161 float Picked_Angle, Last_Picked_Angle;
162 int Max_Speed_For_Picked_Angle;
163
164 // Radius of dis-allowed circles, either side of the robot, which
165 // we can't enter due to our minimum turning radius.
166 float Blocked_Circle_Radius;
167
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; // millimetres
172 std::vector<std::vector<float> > Cell_Enlarge;
173
174 // Cell_Sector[x][y] is a vector of indices to sectors that are effected if cell (x,y) contains
175 // an obstacle.
176 // Cell enlargement is taken into account.
177 // Acess as: Cell_Sector[speed_index][x][y][sector_index]
178 std::vector<std::vector<std::vector<std::vector<int> > > > Cell_Sector;
179 std::vector<float> Candidate_Angle;
180 std::vector<int> Candidate_Speed;
181
182 double dist_eps;
183 double ang_eps;
184
185 float *Last_Binary_Hist;
186
187 // Minimum turning radius at different speeds, in millimeters
188 std::vector<int> Min_Turning_Radius;
189
190 // Keep track of last update, so we can monitor acceleration
191 timeval last_update_time;
192
193 int last_chosen_speed;
194};
195
196#endif
Definition vfh_algorithm.h:28