21 #ifndef VFH_ALGORITHM_H
22 #define VFH_ALGORITHM_H
25 #include <libplayercore/playercore.h>
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;
T min(T a, T b)
Return the minimum of a, b.
Definition: utility.h:91
#define PLAYER_WARN1(msg, a)
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)
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)
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:104