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:109
Definition: player_interfaces.h:3039
#define PLAYER_PLANNER_CMD_GOAL
Command subtype: set goal position.
Definition: player_interfaces.h:3129
uint8_t state
FALSE for off, TRUE for on.
Definition: player_interfaces.h:667
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:89
position 2d velocity command
Definition: player_interfaces.h:617
uint32_t data_count
The number of cells.
Definition: player_interfaces.h:3071
char * name
Identifier for the geometric shape.
Definition: player_interfaces.h:5153
uint32_t data_count
Size of data as stored in buffer (bytes)
Definition: player_interfaces.h:3452
player_pose3d_t * element_poses
Pose of each individual element that makes up the device (in device CS).
Definition: player_interfaces.h:5014
player_pose2d_t goal
Goal location (m,m,rad)
Definition: player_interfaces.h:3173
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:620
Definition: vfh_algorithm.h:27
float miny
The minimum and maximum coordinates of all the line segments [meters].
Definition: player_interfaces.h:3089
#define PLAYER_LASER_DATA_SCAN
Data subtype: scan.
Definition: player_interfaces.h:845
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
uint32_t poses_count
The number of valid poses.
Definition: player_interfaces.h:788
player_vectormap_feature_data_t * features
Array of map features.
Definition: player_interfaces.h:5185
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
float maxy
The minimum and maximum coordinates of all the line segments [meters].
Definition: player_interfaces.h:3091
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:771
Data AND Request/reply: geometry.
Definition: player_interfaces.h:785
int8_t * data
Cell occupancy value.
Definition: player_interfaces.h:3075
char * name
Identifier for the layer.
Definition: player_interfaces.h:5170
Generic message header.
Definition: player.h:160
unsigned int GetDataSize()
Size of message data.
Definition: message.h:189
uint32_t layers_count
The number of layers.
Definition: player_interfaces.h:5194
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:657
#define PLAYER_PLANNER_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:3126
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:663
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:165
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:73
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
#define PLAYER_POSITION2D_REQ_MOTOR_POWER
Request/reply: Motor power.
Definition: player_interfaces.h:496
uint32_t name_count
Length of name in bytes.
Definition: player_interfaces.h:5179
position2d power config
Definition: player_interfaces.h:664
double px
X [m].
Definition: player.h:219
struct player_vectormap_layer_data player_vectormap_layer_data_t
Vectormap data.
A line segment, used to construct vector-based maps.
Definition: player.h:290
double x1
Endpoints [m].
Definition: player.h:313
Request/reply: get grid map tile.
Definition: player_interfaces.h:3060
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
Vectormap data.
Definition: player_interfaces.h:5176
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:167
uint32_t height
The size of the map [pixels].
Definition: player_interfaces.h:3046
uint8_t state
Motor state (FALSE is either off or locked, depending on the driver).
Definition: player_interfaces.h:633
virtual void Main(void)=0
Main method for driver thread.
#define PLAYER_RANGER_DATA_RANGE
Data subtype: range scan.
Definition: player_interfaces.h:4949
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
struct player_position2d_geom player_position2d_geom_t
position2d geom
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:187
Data: state (PLAYER_PLANNER_DATA_STATE)
Definition: player_interfaces.h:3147
#define PLAYER_OPAQUE_DATA_STATE
Data subtype: generic state.
Definition: player_interfaces.h:3434
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
double py
Y [m].
Definition: player.h:221
float scale
The scale of the map [m/pixel].
Definition: player_interfaces.h:3042
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:5027
float minx
The minimum and maximum coordinates of all the line segments [meters].
Definition: player_interfaces.h:3085
#define PLAYER_WARN2(msg, a, b)
Error message macros.
Definition: error.h:90
float maxx
The minimum and maximum coordinates of all the line segments [meters].
Definition: player_interfaces.h:3087
#define PLAYER_VECTORMAP_REQ_GET_LAYER_DATA
Request/reply subtype: get layer data.
Definition: player_interfaces.h:5140
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
float * ranges
Range readings [m].
Definition: player_interfaces.h:896
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Request/reply: Turn power on/off (PLAYER_RANGER_REQ_POWER)
Definition: player_interfaces.h:5082
uint32_t row
The tile origin [pixels].
Definition: player_interfaces.h:3065
Data and Request/reply: Get geometry.
Definition: player_interfaces.h:5005
player_segment_t * segments
Line segments.
Definition: player_interfaces.h:3095
uint32_t width
The size of the tile [pixels].
Definition: player_interfaces.h:3067
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
uint32_t height
The size of the tile [pixels].
Definition: player_interfaces.h:3069
double x0
Origin x [m].
Definition: player.h:309
Vectormap info.
Definition: player_interfaces.h:5189
#define PLAYER_SONAR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:761
#define PLAYER_RANGER_REQ_POWER
Request/reply subtype: power config.
Definition: player_interfaces.h:4967
double sl
Length [m].
Definition: player.h:258
int8_t data_range
Maximum value for each cell (-range <= EMPTY < 0, unknown = 0, 0 < OCCUPIED <= range)
Definition: player_interfaces.h:3073
uint32_t width
The size of the map [pixels].
Definition: player_interfaces.h:3044
uint32_t wkb_count
Length of data in bytes.
Definition: player_interfaces.h:5155
virtual void Update()
Update non-threaded drivers.
Definition: driver.h:676
float * ranges
The range readings [m].
Definition: player_interfaces.h:776
Data: range scan (PLAYER_RANGER_DATA_RANGE)
Definition: player_interfaces.h:5024
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
uint32_t element_poses_count
Number of individual elements that make up the device.
Definition: player_interfaces.h:5012
virtual int Shutdown()
Finalize the driver.
virtual int Setup()
Initialize the driver.
#define PLAYER_SONAR_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:755
Class for loading configuration file information.
Definition: configfile.h:195
#define PLAYER_MAP_REQ_GET_DATA
Request/reply subtype: get grid map tile
Definition: player_interfaces.h:3024
double sw
Width [m].
Definition: player.h:256
uint32_t col
The tile origin [pixels].
Definition: player_interfaces.h:3063
A device address.
Definition: player.h:144
An autopointer for the message queue.
Definition: message.h:72
double y0
Origin y [m].
Definition: player.h:311
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:611
char * name
Identifier for the layer.
Definition: player_interfaces.h:5181
#define PLAYER_VECTORMAP_REQ_GET_MAP_INFO
Request/reply subtype: get vectormap meta-data.
Definition: player_interfaces.h:5137
uint32_t name_count
Length of name in bytes.
Definition: player_interfaces.h:5151
position2d data
Definition: player_interfaces.h:606
position2d geom
Definition: player_interfaces.h:655
A pose in space.
Definition: player.h:227
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:80
float min_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:886
Base class for drivers which oeprate with a thread.
Definition: driver.h:551
uint32_t ranges_count
The number of valid range readings.
Definition: player_interfaces.h:774
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
struct player_vectormap_info player_vectormap_info_t
Vectormap info.
player_pose3d_t * poses
Pose of each sonar, in robot cs.
Definition: player_interfaces.h:790
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:169
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
player_pose2d_t origin
The origin of the map [m, m, rad].
Definition: player_interfaces.h:3049
#define PLAYER_POSITION2D_CMD_POS
Command: position (PLAYER_POSITION2D_CMD_POS)
Definition: player_interfaces.h:588
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:173
uint32_t features_count
The number of map features.
Definition: player_interfaces.h:5183
Reference-counted message objects.
Definition: message.h:131
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:88
double * ranges
Range readings [m].
Definition: player_interfaces.h:5029
uint8_t * wkb
Well known binary describing the geometric shape.
Definition: player_interfaces.h:5157
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
data
Definition: player_interfaces.h:3449
double y1
Endpoints [m].
Definition: player.h:315
double pa
yaw [rad]
Definition: player.h:223
Base class for all drivers.
Definition: driver.h:107
uint8_t state
Motor state (FALSE is either off or locked, depending on the driver).
Definition: player_interfaces.h:622
uint8_t stall
Are the motors stalled?
Definition: player_interfaces.h:613
player_bbox3d_t size
Dimensions of the base (m).
Definition: player_interfaces.h:660
float resolution
Angular resolution [rad].
Definition: player_interfaces.h:890
Request/reply: get vector map.
Definition: player_interfaces.h:3082
#define PLAYER_MAP_REQ_GET_VECTOR
Request/reply subtype: get vector map.
Definition: player_interfaces.h:3027
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:894
player_extent2d_t extent
Boundary area.
Definition: player_interfaces.h:5198
uint32_t segments_count
The number of line segments
Definition: player_interfaces.h:3093
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:185
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:163
#define PLAYER_MAP_REQ_GET_INFO
Request/reply subtype: get grid map metadata
Definition: player_interfaces.h:3021
#define PLAYER_RANGER_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:4964
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
position2d position command
Definition: player_interfaces.h:626
T max(T a, T b)
Return the maximum of a, b.
Definition: utility.h:122
uint8_t * data
The data we will be sending.
Definition: player_interfaces.h:3454
Command: start or goal position (PLAYER_PLANNER_CMD_GOAL, PLAYER_PLANNER_CMD_START)
Definition: player_interfaces.h:3170
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:629
player_vectormap_layer_info_t * layers
Array of layers.
Definition: player_interfaces.h:5196