Hybrid A* Planner
 All Classes Namespaces Files Functions Variables Friends Pages
constants.h
Go to the documentation of this file.
1 #ifndef CONSTANTS
2 #define CONSTANTS
3 
8 // HEADING => 0 - 359 degrees, 0 being north pointing towards positive Y
12 // X-COORDINATE => designating the width of the grid
13 // Y-COORDINATE => designating the height of the grid
14 
15 #include <cmath>
16 
22 namespace HybridAStar {
27 namespace Constants {
28 // _________________
29 // CONFIG FLAGS
30 
32 static const bool coutDEBUG = 1;
34 static const bool manual = 1;
36 static const bool visualization = 0 * manual;
38 static const bool visualization2D = 0* manual;
40 static const bool reverse = true;
42 static const float dubinsShotDistance = 1*1;
44 static const float reedsSheppShotDistance = 1*1;
46 static const bool reedsShepp = true;
48 static const bool dubins = false;
54 static const bool dubinsLookup = false * dubins;
56 static const bool twoD = true;
57 
58 // _________________
59 // GENERAL CONSTANTS
60 
62 static const int iterations = 1000000;
64 static const double bloating = 0.1;
66 static const double width = 1.75 + 2 * bloating;
68 static const double length = 2.65 + 2 * bloating;
70 static const float r = 6;
72 static const int headings = 72;
74 static const float deltaHeadingDeg = 360 / (float)headings;
76 static const float deltaHeadingRad = 2 * M_PI / (float)headings;
78 static const float deltaHeadingNegRad = 2 * M_PI - deltaHeadingRad;
80 static const float cellSize = 1;
89 static const float tieBreaker = 0.01;
90 
91 // ___________________
92 // HEURISTIC CONSTANTS
93 
95 static const float factor2D = sqrt(5) / sqrt(2) + 1;
97 static const float penaltyTurning = 1.03;
99 static const float penaltyReversing = 2.0;
101 static const float penaltyCOD = 2.0;
103 static const float dubinsStepSize = 0.5;
104 
105 
106 // ______________________
107 // DUBINS LOOKUP SPECIFIC
108 
110 static const int dubinsWidth = 15;
112 static const int dubinsArea = dubinsWidth * dubinsWidth;
113 
114 
115 // _________________________
116 // COLLISION LOOKUP SPECIFIC
117 
119 static const int bbSize = std::ceil((sqrt(width * width + length* length) + 4) / cellSize);
121 static const int positionResolution = 10;
123 static const int positions = positionResolution * positionResolution;
125 struct relPos {
127  int x;
129  int y;
130 };
132 struct config {
134  int length;
140  relPos pos[64];
141 };
142 
143 // _________________
144 // SMOOTHER SPECIFIC
146 static const float minRoadWidth = 2;
147 
148 // ____________________________________________
149 // COLOR DEFINITIONS FOR VISUALIZATION PURPOSES
151 struct color {
153  float red;
155  float green;
157  float blue;
158 };
160 static constexpr color teal = {102.f / 255.f, 217.f / 255.f, 239.f / 255.f};
162 static constexpr color green = {166.f / 255.f, 226.f / 255.f, 46.f / 255.f};
164 static constexpr color orange = {253.f / 255.f, 151.f / 255.f, 31.f / 255.f};
166 static constexpr color pink = {249.f / 255.f, 38.f / 255.f, 114.f / 255.f};
168 static constexpr color purple = {174.f / 255.f, 129.f / 255.f, 255.f / 255.f};
169 }
170 }
171 
172 #endif // CONSTANTS
173 
relPos pos[64]
The maximum number of occupied cells.
Definition: constants.h:140
A structure to express colors in RGB values.
Definition: constants.h:151
A structure describing the relative position of the occupied cell based on the center of the vehicle...
Definition: constants.h:125
float blue
the blue portion of the color
Definition: constants.h:157
int length
the number of cells occupied by this configuration of the vehicle
Definition: constants.h:134
A structure capturing the lookup for each theta configuration.
Definition: constants.h:132
float red
the red portion of the color
Definition: constants.h:153
int x
the x position relative to the center
Definition: constants.h:127
float green
the green portion of the color
Definition: constants.h:155
int y
the y position relative to the center
Definition: constants.h:129