-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathlucciOA.h
183 lines (157 loc) · 5.3 KB
/
lucciOA.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
/**
* \file lucciOA.h
* Author: erupter
*
* Created on October 29, 2012, 12:01 PM
*/
#ifndef LUCCIOA_H
#define LUCCIOA_H
#ifdef __cplusplus
extern "C" {
#endif
#include <malloc.h>
#include <math.h>
#include <sys/param.h>
#include "lucciSERVICE.h"
/** \defgroup ObstacleAvoidance */
/**
* @brief Custom PI definition
*
* \ingroup ObstacleAvoidance
*/
#define PI 3.14159265
#define OA_SENSOR_CLOCKWISE 0
#define OA_SENSOR_ANTI_CLOCKWISE 1
//#define OA_GRID_RESOLUTION 0.1 //in meters
/** \brief Active distance
*
* \ingroup ObstacleAvoidance
* Meters. Distance at which the obstacle avoidance becomes active.
*
*/
#define OA_ACTIVE_DISTANCE 7.0 //in meters
/** \brief Grid radial sampling
*
* \ingroup ObstacleAvoidance
* Adimensional. Maximum intensity of the detected occupation of the sector to allow attempting to consider the direction viable.
*
*/
#define OA_GRID_RADIAL_SAMPLING 5 //in degrees
/** \brief Sensor resolution
*
* \ingroup ObstacleAvoidance
* Radians. Incremental radial distance between consecutive laser samples.
*
*/
#define OA_SENSOR_RESOLUTION 0.004363323 //in radians
/** \brief Sensor center
*
* \ingroup ObstacleAvoidance
* In radians. The center of the laser readings relative to the rover.
* If a 180° FOV laser is used, than most probably this is 0.
*
*/
#define OA_SENSOR_CENTER 0.0 //in radians
/** \brief Sensor count
*
* \ingroup ObstacleAvoidance
* Number of readings the laser outputs.
*
*/
#define OA_SENSOR_COUNT 1080 // number of laser readings
/** \brief Window Threshold
*
* \ingroup ObstacleAvoidance
* Adimensional. Maximum intensity of the detected occupation of the sector to allow attempting to consider the direction viable.
*
*/
#define OA_WINDOW_THRESHOLD 5 // value at which to consider a space viable
/** \brief Window Size
*
* \ingroup ObstacleAvoidance
* Adimensional. Number of sequencial radial sectors that must be free to consider that a viable direction.
*
*/
#define OA_WINDOW_SIZE 5 // minimum number of consecutive under threshold values to consider a window
/** \brief Laser sensor data sequence (CW/CCW)
*
* \ingroup ObstacleAvoidance
* Set to reflect how the laser scanner reports its data: CW (human) or CCW (mathematical)
*
*/
#define OA_SENSOR_ORIENTATION OA_SENSOR_CLOCKWISE // or OA_SENSOR_ANTI_CLOCKWISE
/** \brief Maximum Speed
*
* \ingroup ObstacleAvoidance
* Maximum speed the OA algorithm will attempt to attain. In m/s.
*
*/
#define OA_SPEED_MAX 2.0 // meters per second
/** \brief Minimum Speed
*
* \ingroup ObstacleAvoidance
* Minimum speed the OA algorithm will attempt to attain. In m/s.
*
*/
#define OA_SPEED_MIN 0.1 // meters per second
/** \brief Speed moderation active distance
*
* \ingroup ObstacleAvoidance
* The distance from any detected obstacle at which the algorithm will start moderating speed.
*
*/
#define OA_SPEED_MODERATION_DIST 3.0 // meters
/** \brief Safety distance
*
* \ingroup ObstacleAvoidance
* Minimum DO-NOT-CROSS distance from any obstacle: should this
* value be approached forward speed will diminish until it get nullified.
*
*/
#define OA_SAFETY_DISTANCE 1.0 // meters
/** \brief Avoiding Intensity
*
* \ingroup ObstacleAvoidance
* Adimensional multiplier used to tweak how the algorithm performs.
* The base calculation remains the same, this value just allows to augment the strength
* of the algorithm's response allowing for a stronger repulsion.
*
*/
#define OA_AVOID_INTENSITY 2.0 // adimensional, multiplier of the avoiding vector
/** \brief OA Init
*
* \ingroup ObstacleAvoidance
* Initializes the algorithm. Mandatory execution before calling its functions.
*
*/
void OA_init (void);
/** \brief Perform avoidance
*
* \ingroup ObstacleAvoidance
* This function must be periodically called to guarantee correct recording of positions.
* It is designed to be integrated in a superloop.
* If the module is disabled, calling this function won't affect performance, as no operations
* will be performed in such a case (the module will check its state and immediately return).
* @param readings vector of RTB_FLOAT_TYPE values carrying the laser readings
* @param count long variable containing the number of laser readings in a vector
* @param yaw RTBvector containing the current yaw (true north) of the vehicle
* @param desired_heading RTBvector containing the desired heading
* @return an RTBvector with the actual commands (heading, speed)
* @see RTBvector
*/
RTBvector OA_perform_avoidance (RTB_FLOAT_TYPE readings[], long count, RTBvector yaw, RTBvector desired_heading);
/**
* @brief Debug define. Set to 1 to enable.
* \ingroup ObstacleAvoidance
*/
#define DEBUG 0
/**
* @brief Simulator define. Set to 1 to enable.
* \ingroup ObstacleAvoidance
* This requires Player 3 and Stage 4 to be installed on the system.
*/
#define SIMUL 0
#ifdef __cplusplus
}
#endif
#endif /* LUCCIOA_H */