APM:Libraries
SmartRTL_test.h
Go to the documentation of this file.
1 #include <vector>
2 
3 #include <AP_BoardConfig/AP_BoardConfig.h>
4 #include <AP_HAL/AP_HAL.h>
5 #include <AP_Math/AP_Math.h>
6 #include <AP_SmartRTL/AP_SmartRTL.h>
7 #include "../../../../ArduCopter/GCS_Copter.h"
8 
9 // vectors defined below:
10 // test_path_before
11 // test_path_after_adding
12 // test_path_after_simplifying
13 // test_path_after_pruning
14 // test_path_complete
15 
16 // assume that any point without a comment should be kept
17 std::vector<Vector3f> test_path_before {
18  {0.0, 0.0, 0.0},
19  {3.0, 0.0, 0.0}, // simplified
20  {3.0, 0.0, 0.0}, // not added
21  {6.0, 0.0, 0.0}, // simplified
22  {10.0, 0.0, 0.0},
23  {10.0, 3.0, 0.0},
24  {13.0, 3.0, 0.0},
25  {13.0, 6.0, 0.0},
26  {16.0, 6.0, 0.0},
27  {16.0, 6.0, 1.0}, // not added
28  {16.0, 8.0, 1.0},
29  {18.0, 8.0, 0.0},
30  {20.0, 10.0, 0.0},
31  {20.0, 10.0, 10.0},
32  {23.0, 10.0, 10.0},
33  {23.0, 13.0, 10.0},
34  {26.0, 13.0, 10.0},
35  {26.0, 16.0, 10.0},
36  {29.0, 16.0, 10.0},
37  {29.0, 19.0, 10.0},
38  {32.0, 19.0, 10.0},
39  {32.0, 22.0, 10.0},
40  {35.0, 22.0, 10.0},
41  {35.0, 25.0, 10.0},
42  {38.0, 25.0, 10.0},
43  {38.0, 28.0, 10.0},
44  {41.0, 28.0, 10.0},
45  {41.0, 31.0, 10.0},
46  {44.0, 31.0, 10.0},
47  {44.0, 34.0, 10.0},
48  {47.0, 34.0, 10.0},
49  {47.0, 37.0, 10.0},
50  {51.0, 37.0, 10.0},
51  {51.0, 41.0, 10.0},
52  {54.0, 41.0, 10.0},
53  {54.0, 44.0, 10.0},
54  {57.0, 44.0, 10.0},
55  {57.0, 47.0, 10.0},
56  {60.0, 47.0, 10.0},
57  {60.0, 40.0, 10.0},
58  {63.0, 40.0, 10.0},
59  {63.0, 43.0, 10.0},
60  {66.0, 43.0, 10.0},
61  {66.0, 46.0, 10.0},
62  {69.0, 46.0, 10.0},
63  {69.0, 49.0, 10.0},
64  {72.0, 49.0, 10.0},
65  {72.0, 52.0, 10.0},
66  {75.0, 52.0, 10.0},
67  {75.0, 55.0, 10.0},
68  {100.0, 100.0, 100.0}, // pruned
69  {103.0, 100.0, 100.0}, // pruned
70  {106.0, 103.0, 100.0}, // pruned
71  {103.0, 106.0, 100.0}, // pruned
72  {100.0, 103.0, 100.0}, // pruned
73  {103.0, 100.0, 100.0}, // pruned
74  {200.0, 200.0, 200.0},
75  {203.0, 200.0, 200.0},
76  {203.0, 203.0, 200.0},
77  {206.0, 203.0, 200.0},
78  {206.0, 206.0, 200.0},
79  {209.0, 206.0, 200.0},
80  {209.0, 209.0, 200.0},
81  {212.0, 209.0, 200.0},
82  {212.0, 212.0, 200.0},
83  {220.0, 220.0, 200.0},
84  {223.0, 220.0, 200.0}, // pruned
85  {226.0, 223.0, 200.0}, // pruned
86  {223.0, 226.0, 200.0}, // pruned
87  {220.0, 223.0, 200.0}, // pruned
88  {223.0, 220.0, 201.0}, // pruned
89  {226.0, 223.0, 200.0}, // pruned
90  {223.0, 226.0, 200.0}, // pruned
91  {220.0, 223.0, 200.0}, // pruned
92  {223.0, 220.0, 199.0}, // pruned
93  {229.0, 220.0, 200.0},
94  {300.0, 300.0, 300.0},
95  {305.0, 300.0, 300.0}, // simplified, pruned
96  {310.0, 300.0, 300.0}, // simplified, pruned
97  {315.0, 300.5, 300.0}, // simplified, pruned
98  {320.0, 299.5, 300.0}, // simplified, pruned
99  {325.0, 300.5, 300.0}, // simplified, pruned
100  {330.0, 299.5, 300.0}, // simplified, pruned
101  {335.0, 300.5, 300.0}, // simplified, pruned
102  {340.0, 299.5, 300.0}, // simplified, pruned
103  {345.0, 300.5, 300.0}, // simplified, pruned
104  {350.0, 300.0, 300.0}, // pruned
105  {350.0, 300.0, 400.0}, // pruned
106  {345.0, 300.5, 400.0}, // simplified, pruned
107  {340.0, 299.5, 400.0}, // simplified, pruned
108  {335.0, 300.5, 400.0}, // simplified, pruned
109  {330.0, 299.5, 400.0}, // simplified, pruned
110  {325.0, 300.5, 400.0}, // simplified, pruned
111  {320.0, 299.5, 400.0}, // simplified, pruned
112  {315.0, 300.5, 400.0}, // simplified, pruned
113  {310.0, 300.0, 400.0}, // simplified, pruned
114  {305.0, 300.0, 400.0}, // pruned
115  {300.0, 300.0, 295.0},
116 };
117 
118 std::vector<Vector3f> test_path_after_adding {
119  {0.0, 0.0, 0.0}, // 0
120  {3.0, 0.0, 0.0},
121  {6.0, 0.0, 0.0},
122  {10.0, 0.0, 0.0},
123  {10.0, 3.0, 0.0},
124  {13.0, 3.0, 0.0},
125  {13.0, 6.0, 0.0},
126  {16.0, 6.0, 0.0},
127  {16.0, 8.0, 1.0},
128  {18.0, 8.0, 0.0},
129  {20.0, 10.0, 0.0}, // 10
130  {20.0, 10.0, 10.0},
131  {23.0, 10.0, 10.0},
132  {23.0, 13.0, 10.0},
133  {26.0, 13.0, 10.0},
134  {26.0, 16.0, 10.0},
135  {29.0, 16.0, 10.0},
136  {29.0, 19.0, 10.0},
137  {32.0, 19.0, 10.0},
138  {32.0, 22.0, 10.0},
139  {35.0, 22.0, 10.0}, // 20
140  {35.0, 25.0, 10.0},
141  {38.0, 25.0, 10.0},
142  {38.0, 28.0, 10.0},
143  {41.0, 28.0, 10.0},
144  {41.0, 31.0, 10.0},
145  {44.0, 31.0, 10.0},
146  {44.0, 34.0, 10.0},
147  {47.0, 34.0, 10.0},
148  {47.0, 37.0, 10.0},
149  {51.0, 37.0, 10.0}, // 30
150  {51.0, 41.0, 10.0},
151  {54.0, 41.0, 10.0},
152  {54.0, 44.0, 10.0},
153  {57.0, 44.0, 10.0},
154  {57.0, 47.0, 10.0},
155  {60.0, 47.0, 10.0},
156  {60.0, 40.0, 10.0},
157  {63.0, 40.0, 10.0},
158  {63.0, 43.0, 10.0},
159  {66.0, 43.0, 10.0}, // 40
160  {66.0, 46.0, 10.0},
161  {69.0, 46.0, 10.0},
162  {69.0, 49.0, 10.0},
163  {72.0, 49.0, 10.0},
164  {72.0, 52.0, 10.0},
165  {75.0, 52.0, 10.0},
166  {75.0, 55.0, 10.0},
167  {100.0, 100.0, 100.0},
168  {103.0, 100.0, 100.0},
169  {106.0, 103.0, 100.0}, // 50
170  {103.0, 106.0, 100.0},
171  {100.0, 103.0, 100.0},
172  {103.0, 100.0, 100.0},
173  {200.0, 200.0, 200.0},
174  {203.0, 200.0, 200.0},
175  {203.0, 203.0, 200.0},
176  {206.0, 203.0, 200.0},
177  {206.0, 206.0, 200.0},
178  {209.0, 206.0, 200.0},
179  {209.0, 209.0, 200.0}, // 60
180  {212.0, 209.0, 200.0},
181  {212.0, 212.0, 200.0},
182  {220.0, 220.0, 200.0},
183  {223.0, 220.0, 200.0},
184  {226.0, 223.0, 200.0},
185  {223.0, 226.0, 200.0},
186  {220.0, 223.0, 200.0},
187  {223.0, 220.0, 201.0},
188  {226.0, 223.0, 200.0},
189  {223.0, 226.0, 200.0}, // 70
190  {220.0, 223.0, 200.0},
191  {223.0, 220.0, 199.0},
192  {229.0, 220.0, 200.0},
193  {300.0, 300.0, 300.0},
194  {305.0, 300.0, 300.0},
195  {310.0, 300.0, 300.0},
196  {315.0, 300.5, 300.0},
197  {320.0, 299.5, 300.0},
198  {325.0, 300.5, 300.0},
199  {330.0, 299.5, 300.0}, // 80
200  {335.0, 300.5, 300.0},
201  {340.0, 299.5, 300.0},
202  {345.0, 300.5, 300.0},
203  {350.0, 300.0, 300.0},
204  {350.0, 300.0, 400.0},
205  {345.0, 300.5, 400.0},
206  {340.0, 299.5, 400.0},
207  {335.0, 300.5, 400.0},
208  {330.0, 299.5, 400.0},
209  {325.0, 300.5, 400.0}, // 90
210  {320.0, 299.5, 400.0},
211  {315.0, 300.5, 400.0},
212  {310.0, 300.0, 400.0},
213  {305.0, 300.0, 400.0},
214  {300.0, 300.0, 295.0},
215 };
216 
217 std::vector<Vector3f> test_path_after_simplifying {
218  {0.0, 0.0, 0.0}, // 0
219  {10.0, 0.0, 0.0},
220  {10.0, 3.0, 0.0},
221  {13.0, 3.0, 0.0},
222  {13.0, 6.0, 0.0},
223  {16.0, 6.0, 0.0},
224  {16.0, 8.0, 1.0},
225  {18.0, 8.0, 0.0},
226  {20.0, 10.0, 0.0},
227  {20.0, 10.0, 10.0},
228  {23.0, 10.0, 10.0}, // 10
229  {23.0, 13.0, 10.0},
230  {26.0, 13.0, 10.0},
231  {26.0, 16.0, 10.0},
232  {29.0, 16.0, 10.0},
233  {29.0, 19.0, 10.0},
234  {32.0, 19.0, 10.0},
235  {32.0, 22.0, 10.0},
236  {35.0, 22.0, 10.0},
237  {35.0, 25.0, 10.0},
238  {38.0, 25.0, 10.0}, // 20
239  {38.0, 28.0, 10.0},
240  {41.0, 28.0, 10.0},
241  {41.0, 31.0, 10.0},
242  {44.0, 31.0, 10.0},
243  {44.0, 34.0, 10.0},
244  {47.0, 34.0, 10.0},
245  {47.0, 37.0, 10.0},
246  {51.0, 37.0, 10.0},
247  {51.0, 41.0, 10.0},
248  {54.0, 41.0, 10.0}, // 30
249  {54.0, 44.0, 10.0},
250  {57.0, 44.0, 10.0},
251  {57.0, 47.0, 10.0},
252  {60.0, 47.0, 10.0},
253  {60.0, 40.0, 10.0},
254  {63.0, 40.0, 10.0},
255  {63.0, 43.0, 10.0},
256  {66.0, 43.0, 10.0},
257  {66.0, 46.0, 10.0},
258  {69.0, 46.0, 10.0}, // 40
259  {69.0, 49.0, 10.0},
260  {72.0, 49.0, 10.0},
261  {72.0, 52.0, 10.0},
262  {75.0, 52.0, 10.0},
263  {75.0, 55.0, 10.0},
264  {100.0, 100.0, 100.0},
265  {103.0, 100.0, 100.0},
266  {106.0, 103.0, 100.0},
267  {103.0, 106.0, 100.0},
268  {100.0, 103.0, 100.0}, // 50
269  {103.0, 100.0, 100.0},
270  {200.0, 200.0, 200.0},
271  {203.0, 200.0, 200.0},
272  {203.0, 203.0, 200.0},
273  {206.0, 203.0, 200.0},
274  {206.0, 206.0, 200.0},
275  {209.0, 206.0, 200.0},
276  {209.0, 209.0, 200.0},
277  {212.0, 209.0, 200.0},
278  {212.0, 212.0, 200.0}, // 60
279  {220.0, 220.0, 200.0},
280  {223.0, 220.0, 200.0},
281  {226.0, 223.0, 200.0},
282  {223.0, 226.0, 200.0},
283  {220.0, 223.0, 200.0},
284  {223.0, 220.0, 201.0},
285  {226.0, 223.0, 200.0},
286  {223.0, 226.0, 200.0},
287  {220.0, 223.0, 200.0},
288  {223.0, 220.0, 199.0}, // 70
289  {229.0, 220.0, 200.0},
290  {300.0, 300.0, 300.0},
291  {350.0, 300.0, 300.0},
292  {350.0, 300.0, 400.0},
293  {305.0, 300.0, 400.0},
294  {300.0, 300.0, 295.0},
295 };
296 
297 std::vector<Vector3f> test_path_complete {
298  {0.0, 0.0, 0.0}, // 0
299  {10.0, 0.0, 0.0},
300  {10.0, 3.0, 0.0},
301  {13.0, 3.0, 0.0},
302  {13.0, 6.0, 0.0},
303  {16.0, 6.0, 0.0},
304  {16.0, 8.0, 1.0},
305  {18.0, 8.0, 0.0},
306  {20.0, 10.0, 0.0},
307  {20.0, 10.0, 10.0},
308  {23.0, 10.0, 10.0}, // 10
309  {23.0, 13.0, 10.0},
310  {26.0, 13.0, 10.0},
311  {26.0, 16.0, 10.0},
312  {29.0, 16.0, 10.0},
313  {29.0, 19.0, 10.0},
314  {32.0, 19.0, 10.0},
315  {32.0, 22.0, 10.0},
316  {35.0, 22.0, 10.0},
317  {35.0, 25.0, 10.0},
318  {38.0, 25.0, 10.0}, // 20
319  {38.0, 28.0, 10.0},
320  {41.0, 28.0, 10.0},
321  {41.0, 31.0, 10.0},
322  {44.0, 31.0, 10.0},
323  {44.0, 34.0, 10.0},
324  {47.0, 34.0, 10.0},
325  {47.0, 37.0, 10.0},
326  {51.0, 37.0, 10.0},
327  {51.0, 41.0, 10.0},
328  {54.0, 41.0, 10.0}, // 30
329  {54.0, 44.0, 10.0},
330  {57.0, 44.0, 10.0},
331  {57.0, 47.0, 10.0},
332  {60.0, 47.0, 10.0},
333  {60.0, 40.0, 10.0},
334  {63.0, 40.0, 10.0},
335  {63.0, 43.0, 10.0},
336  {66.0, 43.0, 10.0},
337  {66.0, 46.0, 10.0},
338  {69.0, 46.0, 10.0}, // 40
339  {69.0, 49.0, 10.0},
340  {72.0, 49.0, 10.0},
341  {72.0, 52.0, 10.0},
342  {75.0, 52.0, 10.0},
343  {75.0, 55.0, 10.0},
344  {100.0, 100.0, 100.0},
345  {103.0, 100.0, 100.0},
346  {103.0, 100.0, 100.0},
347  {200.0, 200.0, 200.0},
348  {203.0, 200.0, 200.0}, // 50
349  {203.0, 203.0, 200.0},
350  {206.0, 203.0, 200.0},
351  {206.0, 206.0, 200.0},
352  {209.0, 206.0, 200.0},
353  {209.0, 209.0, 200.0},
354  {212.0, 209.0, 200.0},
355  {212.0, 212.0, 200.0},
356  {220.0, 220.0, 200.0},
357  {223.0, 220.0, 200.0},
358  {223.2368474, 220.0789542, 199.5263052}, // 60
359  {229.0, 220.0, 200.0},
360  {300.1223662, 300.0, 300.0696305},
361  {300.0, 300.0, 295.0},
362 };
test_path_complete
std::vector< Vector3f > test_path_complete
Definition: SmartRTL_test.h:297
AP_HAL.h
test_path_after_adding
std::vector< Vector3f > test_path_after_adding
Definition: SmartRTL_test.h:118
test_path_before
std::vector< Vector3f > test_path_before
Definition: SmartRTL_test.h:17
AP_BoardConfig.h
AP_Math.h
AP_SmartRTL.h
test_path_after_simplifying
std::vector< Vector3f > test_path_after_simplifying
Definition: SmartRTL_test.h:217
Generated on Sun Jun 17 2018 14:18:51 for APM:Libraries by   doxygen 1.8.13