Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AC_Avoidance_config.h"
17 :
18 : #if AP_OAPATHPLANNER_ENABLED
19 :
20 : #include "AP_OAPathPlanner.h"
21 : #include <AP_Math/AP_Math.h>
22 : #include <AP_AHRS/AP_AHRS.h>
23 : #include <AC_Fence/AC_Fence.h>
24 : #include <AP_Logger/AP_Logger.h>
25 : #include "AP_OABendyRuler.h"
26 : #include "AP_OADijkstra.h"
27 :
28 : extern const AP_HAL::HAL &hal;
29 :
30 : // parameter defaults
31 : static constexpr float OA_MARGIN_MAX_DEFAULT = 5;
32 : static constexpr int16_t OA_OPTIONS_DEFAULT = 1;
33 :
34 : static constexpr int16_t OA_UPDATE_MS = 1000; // path planning updates run at 1hz
35 : static constexpr int16_t OA_TIMEOUT_MS = 3000; // results over 3 seconds old are ignored
36 :
37 : const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = {
38 :
39 : // @Param: TYPE
40 : // @DisplayName: Object Avoidance Path Planning algorithm to use
41 : // @Description: Enabled/disable path planning around obstacles
42 : // @Values: 0:Disabled,1:BendyRuler,2:Dijkstra,3:Dijkstra with BendyRuler
43 : // @User: Standard
44 : AP_GROUPINFO_FLAGS("TYPE", 1, AP_OAPathPlanner, _type, OA_PATHPLAN_DISABLED, AP_PARAM_FLAG_ENABLE),
45 :
46 : // Note: Do not use Index "2" for any new parameter
47 : // It was being used by _LOOKAHEAD which was later moved to AP_OABendyRuler
48 :
49 : // @Param: MARGIN_MAX
50 : // @DisplayName: Object Avoidance wide margin distance
51 : // @Description: Minimum distance in meters that the vehicle will try to stay away from obstacles and fences. BendyRuler only accepts a candidate path whose clearance exceeds this, and Dijkstra uses it as the margin held from fences. This is not a detection range: use OA_BR_LOOKAHEAD to limit how far ahead BendyRuler looks for obstacles.
52 : // @Units: m
53 : // @Range: 0.1 100
54 : // @Increment: 1
55 : // @User: Standard
56 : AP_GROUPINFO("MARGIN_MAX", 3, AP_OAPathPlanner, _margin_max, OA_MARGIN_MAX_DEFAULT),
57 :
58 : // @Group: DB_
59 : // @Path: AP_OADatabase.cpp
60 : AP_SUBGROUPINFO(_oadatabase, "DB_", 4, AP_OAPathPlanner, AP_OADatabase),
61 :
62 : // @Param: OPTIONS
63 : // @DisplayName: Options while recovering from Object Avoidance
64 : // @Description: Bitmask which will govern vehicles behaviour while recovering from Obstacle Avoidance (i.e Avoidance is turned off after the path ahead is clear).
65 : // @Bitmask{Rover}: 0: Reset the origin of the waypoint to the present location, 1: log Dijkstra points
66 : // @Bitmask{Copter}: 1:log Dijkstra points, 2:Allow fast waypoints (Dijkastras only)
67 : // @User: Standard
68 : AP_GROUPINFO("OPTIONS", 5, AP_OAPathPlanner, _options, OA_OPTIONS_DEFAULT),
69 :
70 : // @Group: BR_
71 : // @Path: AP_OABendyRuler.cpp
72 : AP_SUBGROUPPTR(_oabendyruler, "BR_", 6, AP_OAPathPlanner, AP_OABendyRuler),
73 :
74 : AP_GROUPEND
75 : };
76 :
77 : /// Constructor
78 1286 : AP_OAPathPlanner::AP_OAPathPlanner()
79 : {
80 1286 : _singleton = this;
81 :
82 1286 : AP_Param::setup_object_defaults(this, var_info);
83 1286 : }
84 :
85 : // perform any required initialisation
86 1284 : void AP_OAPathPlanner::init()
87 : {
88 : // run background task looking for best alternative destination
89 1284 : switch (_type) {
90 1275 : case OA_PATHPLAN_DISABLED:
91 : // do nothing
92 1275 : return;
93 2 : case OA_PATHPLAN_BENDYRULER:
94 2 : if (_oabendyruler == nullptr) {
95 2 : _oabendyruler = NEW_NOTHROW AP_OABendyRuler();
96 2 : AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info);
97 : }
98 2 : break;
99 7 : case OA_PATHPLAN_DIJKSTRA:
100 : #if AP_FENCE_ENABLED
101 7 : if (_oadijkstra == nullptr) {
102 7 : _oadijkstra = NEW_NOTHROW AP_OADijkstra(_options);
103 : }
104 : #endif
105 7 : break;
106 0 : case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
107 : #if AP_FENCE_ENABLED
108 0 : if (_oadijkstra == nullptr) {
109 0 : _oadijkstra = NEW_NOTHROW AP_OADijkstra(_options);
110 : }
111 : #endif
112 0 : if (_oabendyruler == nullptr) {
113 0 : _oabendyruler = NEW_NOTHROW AP_OABendyRuler();
114 0 : AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info);
115 : }
116 0 : break;
117 : }
118 :
119 9 : _oadatabase.init();
120 9 : start_thread();
121 : }
122 :
123 : // pre-arm checks that algorithms have been initialised successfully
124 107942 : bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
125 : {
126 : // check if initialisation has succeeded
127 107942 : switch (_type) {
128 107401 : case OA_PATHPLAN_DISABLED:
129 : // do nothing
130 107401 : break;
131 174 : case OA_PATHPLAN_BENDYRULER:
132 174 : if (_oabendyruler == nullptr) {
133 6 : hal.util->snprintf(failure_msg, failure_msg_len, "BendyRuler OA requires reboot");
134 6 : return false;
135 : }
136 168 : break;
137 367 : case OA_PATHPLAN_DIJKSTRA:
138 367 : if (_oadijkstra == nullptr) {
139 7 : hal.util->snprintf(failure_msg, failure_msg_len, "Dijkstra OA requires reboot");
140 7 : return false;
141 : }
142 360 : break;
143 0 : case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
144 0 : if(_oadijkstra == nullptr || _oabendyruler == nullptr) {
145 0 : hal.util->snprintf(failure_msg, failure_msg_len, "OA requires reboot");
146 0 : return false;
147 : }
148 0 : break;
149 : }
150 107929 : return true;
151 : }
152 :
153 9 : bool AP_OAPathPlanner::start_thread()
154 : {
155 9 : WITH_SEMAPHORE(_rsem);
156 :
157 9 : if (_thread_created) {
158 0 : return true;
159 : }
160 9 : if (_type == OA_PATHPLAN_DISABLED) {
161 0 : return false;
162 : }
163 :
164 : // create the avoidance thread as low priority. It should soak
165 : // up spare CPU cycles to fill in the avoidance_result structure based
166 : // on requests in avoidance_request
167 9 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OAPathPlanner::avoidance_thread, void),
168 : "avoidance",
169 : 8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
170 0 : return false;
171 : }
172 9 : _thread_created = true;
173 9 : return true;
174 9 : }
175 :
176 : // helper function to map OABendyType to OAPathPlannerUsed
177 296 : AP_OAPathPlanner::OAPathPlannerUsed AP_OAPathPlanner::map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type)
178 : {
179 296 : switch (bendy_type) {
180 296 : case AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL:
181 296 : return OAPathPlannerUsed::BendyRulerHorizontal;
182 :
183 0 : case AP_OABendyRuler::OABendyType::OA_BENDY_VERTICAL:
184 0 : return OAPathPlannerUsed::BendyRulerVertical;
185 :
186 0 : default:
187 : case AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED:
188 0 : return OAPathPlannerUsed::None;
189 : }
190 : }
191 :
192 : // provides an alternative target location if path planning around obstacles is required
193 : // returns true and updates result_origin, result_destination, result_next_destination with an intermediate path
194 : // result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear (only supported by Dijkstras)
195 : // path_planner_used updated with which path planner produced the result
196 3616575 : AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location ¤t_loc,
197 : const Location &origin,
198 : const Location &destination,
199 : const Location &next_destination,
200 : Location &result_origin,
201 : Location &result_destination,
202 : Location &result_next_destination,
203 : bool &result_dest_to_next_dest_clear,
204 : OAPathPlannerUsed &path_planner_used)
205 : {
206 : // exit immediately if disabled or thread is not running from a failed init
207 3616575 : if (_type == OA_PATHPLAN_DISABLED || !_thread_created) {
208 3588220 : return OA_NOT_REQUIRED;
209 : }
210 :
211 : // check if just activated to avoid initial timeout error
212 28355 : const uint32_t now = AP_HAL::millis();
213 28355 : if (now - _last_update_ms > 200) {
214 3 : _activated_ms = now;
215 : }
216 28355 : _last_update_ms = now;
217 :
218 28355 : WITH_SEMAPHORE(_rsem);
219 :
220 : // place new request for the thread to work on
221 28355 : avoidance_request.current_loc = current_loc;
222 28355 : avoidance_request.origin = origin;
223 28355 : avoidance_request.destination = destination;
224 28355 : avoidance_request.next_destination = next_destination;
225 28355 : avoidance_request.ground_speed_vec = AP::ahrs().groundspeed_vector();
226 28355 : avoidance_request.request_time_ms = now;
227 :
228 : // check result's destination and next_destination matches our request
229 : // e.g. check this result was using our current inputs and not from an old request
230 28355 : const bool destination_matches = destination.same_latlon_as(avoidance_result.destination);
231 28355 : const bool next_destination_matches = next_destination.same_latlon_as(avoidance_result.next_destination);
232 :
233 : // check results have not timed out
234 28355 : const bool timed_out = (now - avoidance_result.result_time_ms > OA_TIMEOUT_MS) && (now - _activated_ms > OA_TIMEOUT_MS);
235 :
236 : // return results from background thread's latest checks
237 28355 : if (destination_matches && next_destination_matches && !timed_out) {
238 : // we have a result from the thread
239 28210 : result_origin = avoidance_result.origin_new;
240 28210 : result_destination = avoidance_result.destination_new;
241 28210 : result_next_destination = avoidance_result.next_destination_new;
242 28210 : result_dest_to_next_dest_clear = avoidance_result.dest_to_next_dest_clear;
243 28210 : path_planner_used = avoidance_result.path_planner_used;
244 28210 : return avoidance_result.ret_state;
245 : }
246 :
247 : // if timeout then path planner is taking too long to respond
248 145 : if (timed_out) {
249 0 : return OA_ERROR;
250 : }
251 :
252 : // background thread is working on a new destination
253 145 : return OA_PROCESSING;
254 28355 : }
255 :
256 : // avoidance thread that continually updates the avoidance_result structure based on avoidance_request
257 9 : void AP_OAPathPlanner::avoidance_thread()
258 : {
259 : // require ekf origin to have been set
260 9 : bool origin_set = false;
261 357 : while (!origin_set) {
262 348 : hal.scheduler->delay(500);
263 348 : Location ekf_origin {};
264 : {
265 348 : WITH_SEMAPHORE(AP::ahrs().get_semaphore());
266 348 : origin_set = AP::ahrs().get_origin(ekf_origin);
267 348 : }
268 : }
269 :
270 : while (true) {
271 :
272 : // if database queue needs attention, service it faster
273 37017 : if (_oadatabase.process_queue()) {
274 1 : hal.scheduler->delay(1);
275 : } else {
276 37016 : hal.scheduler->delay(20);
277 : }
278 :
279 37008 : const uint32_t now = AP_HAL::millis();
280 37008 : if (now - avoidance_latest_ms < OA_UPDATE_MS) {
281 36442 : continue;
282 : }
283 780 : avoidance_latest_ms = now;
284 :
285 780 : _oadatabase.update();
286 :
287 : // values returned by path planners
288 780 : Location origin_new;
289 780 : Location destination_new;
290 780 : Location next_destination_new;
291 780 : bool dest_to_next_dest_clear = false;
292 : {
293 780 : WITH_SEMAPHORE(_rsem);
294 780 : if (avoidance_request.request_time_ms == 0 ||
295 571 : now - avoidance_request.request_time_ms > OA_TIMEOUT_MS) {
296 : // no request has ever been posted, or this is a very old request, don't process it
297 211 : continue;
298 : }
299 :
300 : // copy request to avoid conflict with main thread
301 569 : avoidance_request2 = avoidance_request;
302 :
303 : // store passed in origin, destination and next_destination so we can return it if object avoidance is not required
304 569 : origin_new = avoidance_request.origin;
305 569 : destination_new = avoidance_request.destination;
306 569 : next_destination_new = avoidance_request.next_destination;
307 780 : }
308 :
309 : // run background task looking for best alternative destination
310 569 : OA_RetState res = OA_NOT_REQUIRED;
311 569 : OAPathPlannerUsed path_planner_used = OAPathPlannerUsed::None;
312 569 : switch (_type) {
313 3 : case OA_PATHPLAN_DISABLED:
314 3 : continue;
315 296 : case OA_PATHPLAN_BENDYRULER: {
316 296 : if (_oabendyruler == nullptr) {
317 0 : continue;
318 : }
319 296 : _oabendyruler->set_config(_margin_max);
320 :
321 : AP_OABendyRuler::OABendyType bendy_type;
322 296 : if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, bendy_type, false)) {
323 199 : res = OA_SUCCESS;
324 : }
325 296 : path_planner_used = map_bendytype_to_pathplannerused(bendy_type);
326 296 : break;
327 : }
328 :
329 270 : case OA_PATHPLAN_DIJKSTRA: {
330 : #if AP_FENCE_ENABLED
331 270 : if (_oadijkstra == nullptr) {
332 0 : continue;
333 : }
334 270 : _oadijkstra->set_fence_margin(_margin_max);
335 270 : const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc,
336 270 : avoidance_request2.destination,
337 270 : avoidance_request2.next_destination,
338 : origin_new,
339 : destination_new,
340 : next_destination_new,
341 : dest_to_next_dest_clear);
342 270 : switch (dijkstra_state) {
343 43 : case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
344 43 : res = OA_NOT_REQUIRED;
345 43 : break;
346 0 : case AP_OADijkstra::DIJKSTRA_STATE_ERROR:
347 0 : res = OA_ERROR;
348 0 : break;
349 227 : case AP_OADijkstra::DIJKSTRA_STATE_SUCCESS:
350 227 : res = OA_SUCCESS;
351 227 : break;
352 : }
353 270 : path_planner_used = OAPathPlannerUsed::Dijkstras;
354 : #endif
355 270 : break;
356 : }
357 :
358 0 : case OA_PATHPLAN_DJIKSTRA_BENDYRULER: {
359 0 : if ((_oabendyruler == nullptr) || _oadijkstra == nullptr) {
360 0 : continue;
361 : }
362 0 : _oabendyruler->set_config(_margin_max);
363 : AP_OABendyRuler::OABendyType bendy_type;
364 0 : if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, bendy_type, proximity_only)) {
365 : // detected a obstacle by vehicle's proximity sensor. Switch avoidance to BendyRuler till obstacle is out of the way
366 0 : proximity_only = false;
367 0 : res = OA_SUCCESS;
368 0 : path_planner_used = map_bendytype_to_pathplannerused(bendy_type);
369 0 : break;
370 : } else {
371 : // cleared all obstacles, trigger Dijkstra's to calculate path based on current deviated position
372 : #if AP_FENCE_ENABLED
373 0 : if (proximity_only == false) {
374 0 : _oadijkstra->recalculate_path();
375 : }
376 : #endif
377 : // only use proximity avoidance now for BendyRuler
378 0 : proximity_only = true;
379 : }
380 : #if AP_FENCE_ENABLED
381 0 : _oadijkstra->set_fence_margin(_margin_max);
382 0 : const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc,
383 0 : avoidance_request2.destination,
384 0 : avoidance_request2.next_destination,
385 : origin_new,
386 : destination_new,
387 : next_destination_new,
388 : dest_to_next_dest_clear);
389 0 : switch (dijkstra_state) {
390 0 : case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
391 0 : res = OA_NOT_REQUIRED;
392 0 : break;
393 0 : case AP_OADijkstra::DIJKSTRA_STATE_ERROR:
394 0 : res = OA_ERROR;
395 0 : break;
396 0 : case AP_OADijkstra::DIJKSTRA_STATE_SUCCESS:
397 0 : res = OA_SUCCESS;
398 0 : break;
399 : }
400 0 : path_planner_used = OAPathPlannerUsed::Dijkstras;
401 : #endif
402 0 : break;
403 : }
404 :
405 3 : } // switch
406 :
407 : {
408 : // give the main thread the avoidance result
409 566 : WITH_SEMAPHORE(_rsem);
410 :
411 : // place the destination and next destination used into the result (used by the caller to verify the result matches their request)
412 566 : avoidance_result.destination = avoidance_request2.destination;
413 566 : avoidance_result.next_destination = avoidance_request2.next_destination;
414 566 : avoidance_result.dest_to_next_dest_clear = dest_to_next_dest_clear;
415 :
416 : // fill the result structure with the intermediate path
417 566 : avoidance_result.origin_new = (res == OA_SUCCESS) ? origin_new : avoidance_result.origin_new;
418 566 : avoidance_result.destination_new = (res == OA_SUCCESS) ? destination_new : avoidance_result.destination;
419 566 : avoidance_result.next_destination_new = (res == OA_SUCCESS) ? next_destination_new : avoidance_result.next_destination;
420 :
421 : // create new avoidance result.dest_to_next_dest_clear field. fill in with results from dijkstras or leave as unknown
422 566 : avoidance_result.result_time_ms = AP_HAL::millis();
423 566 : avoidance_result.path_planner_used = path_planner_used;
424 566 : avoidance_result.ret_state = res;
425 566 : }
426 37008 : }
427 : }
428 :
429 : // singleton instance
430 : AP_OAPathPlanner *AP_OAPathPlanner::_singleton;
431 :
432 : namespace AP {
433 :
434 0 : AP_OAPathPlanner *ap_oapathplanner()
435 : {
436 0 : return AP_OAPathPlanner::get_singleton();
437 : }
438 :
439 : }
440 :
441 : #endif // AP_OAPATHPLANNER_ENABLED
|
