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 {
29 public:
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 
50  ~VFH_Algorithm();
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 
90 private:
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
T min(T a, T b)
Return the minimum of a, b.
Definition: utility.h:113
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:90
Definition: vfh_algorithm.h:28
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Generic message header.
Definition: player.h:162
unsigned int GetDataSize()
Size of message data.
Definition: message.h:190
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:75
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
A line segment, used to construct vector-based maps.
Definition: player.h:292
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
virtual void Main(void)=0
Main method for driver thread.
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
int ReadInt(int section, const char *name, int value)
Read an integer value.
double ReadLength(int section, const char *name, double value)
Read a length (includes unit conversion, if any).
void * GetPayload()
Get pointer to payload.
Definition: message.h:188
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
#define PLAYER_WARN2(msg, a, b)
Error message macros.
Definition: error.h:91
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
virtual void Update()
Update non-threaded drivers.
Definition: driver.h:677
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
virtual int Shutdown()
Finalize the driver.
virtual int Setup()
Initialize the driver.
Class for loading configuration file information.
Definition: configfile.h:197
A device address.
Definition: player.h:146
An autopointer for the message queue.
Definition: message.h:74
A pose in space.
Definition: player.h:229
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:170
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
Reference-counted message objects.
Definition: message.h:133
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
Base class for all drivers.
Definition: driver.h:109
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:186
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:164
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76
T max(T a, T b)
Return the maximum of a, b.
Definition: utility.h:126