libraries/AC_Avoidance/AP_OAPathPlanner.cpp

   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: Object Avoidance will ignore objects more than this many meters from vehicle
  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 AP_OAPathPlanner::AP_OAPathPlanner()
  79 {
  80     _singleton = this;
  81
  82     AP_Param::setup_object_defaults(this, var_info);
  83 }
  84
  85 // perform any required initialisation
  86 void AP_OAPathPlanner::init()
  87 {
  88     // run background task looking for best alternative destination
  89     switch (_type) {
  90     case OA_PATHPLAN_DISABLED:
  91         // do nothing
  92         return;
  93     case OA_PATHPLAN_BENDYRULER:
  94         if (_oabendyruler == nullptr) {
  95             _oabendyruler = NEW_NOTHROW AP_OABendyRuler();
  96             AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info);
  97         }
  98         break;
  99     case OA_PATHPLAN_DIJKSTRA:
 100 #if AP_FENCE_ENABLED
 101         if (_oadijkstra == nullptr) {
 102             _oadijkstra = NEW_NOTHROW AP_OADijkstra(_options);
 103         }
 104 #endif
 105         break;
 106     case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
 107 #if AP_FENCE_ENABLED
 108         if (_oadijkstra == nullptr) {
 109             _oadijkstra = NEW_NOTHROW AP_OADijkstra(_options);
 110         }
 111 #endif
 112         if (_oabendyruler == nullptr) {
 113             _oabendyruler = NEW_NOTHROW AP_OABendyRuler();
 114             AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info);
 115         }
 116         break;
 117     }
 118
 119     _oadatabase.init();
 120     start_thread();
 121 }
 122
 123 // pre-arm checks that algorithms have been initialised successfully
 124 bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
 125 {
 126     // check if initialisation has succeeded
 127     switch (_type) {
 128     case OA_PATHPLAN_DISABLED:
 129         // do nothing
 130         break;
 131     case OA_PATHPLAN_BENDYRULER:
 132         if (_oabendyruler == nullptr) {
 133             hal.util->snprintf(failure_msg, failure_msg_len, "BendyRuler OA requires reboot");
 134             return false;
 135         }
 136         break;
 137     case OA_PATHPLAN_DIJKSTRA:
 138         if (_oadijkstra == nullptr) {
 139             hal.util->snprintf(failure_msg, failure_msg_len, "Dijkstra OA requires reboot");
 140             return false;
 141         }
 142         break;
 143     case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
 144         if(_oadijkstra == nullptr || _oabendyruler == nullptr) {
 145             hal.util->snprintf(failure_msg, failure_msg_len, "OA requires reboot");
 146             return false;
 147         }
 148         break;
 149     }
 150     return true;
 151 }
 152
 153 bool AP_OAPathPlanner::start_thread()
 154 {
 155     WITH_SEMAPHORE(_rsem);
 156
 157     if (_thread_created) {
 158         return true;
 159     }
 160     if (_type == OA_PATHPLAN_DISABLED) {
 161         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     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OAPathPlanner::avoidance_thread, void),
 168                                       "avoidance",
 169                                       8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
 170         return false;
 171     }
 172     _thread_created = true;
 173     return true;
 174 }
 175
 176 // helper function to map OABendyType to OAPathPlannerUsed
 177 AP_OAPathPlanner::OAPathPlannerUsed AP_OAPathPlanner::map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type)
 178 {
 179     switch (bendy_type) {
 180     case AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL:
 181         return OAPathPlannerUsed::BendyRulerHorizontal;
 182
 183     case AP_OABendyRuler::OABendyType::OA_BENDY_VERTICAL:
 184         return OAPathPlannerUsed::BendyRulerVertical;
 185
 186     default:
 187     case AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED:
 188         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 AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location &current_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     if (_type == OA_PATHPLAN_DISABLED || !_thread_created) {
 208         return OA_NOT_REQUIRED;
 209     }
 210
 211     // check if just activated to avoid initial timeout error
 212     const uint32_t now = AP_HAL::millis();
 213     if (now - _last_update_ms > 200) {
 214         _activated_ms = now;
 215     }
 216     _last_update_ms = now;
 217
 218     WITH_SEMAPHORE(_rsem);
 219
 220     // place new request for the thread to work on
 221     avoidance_request.current_loc = current_loc;
 222     avoidance_request.origin = origin;
 223     avoidance_request.destination = destination;
 224     avoidance_request.next_destination = next_destination;
 225     avoidance_request.ground_speed_vec = AP::ahrs().groundspeed_vector();
 226     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     const bool destination_matches = destination.same_latlon_as(avoidance_result.destination);
 231     const bool next_destination_matches = next_destination.same_latlon_as(avoidance_result.next_destination);
 232
 233     // check results have not timed out
 234     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     if (destination_matches && next_destination_matches && !timed_out) {
 238         // we have a result from the thread
 239         result_origin = avoidance_result.origin_new;
 240         result_destination = avoidance_result.destination_new;
 241         result_next_destination = avoidance_result.next_destination_new;
 242         result_dest_to_next_dest_clear = avoidance_result.dest_to_next_dest_clear;
 243         path_planner_used = avoidance_result.path_planner_used;
 244         return avoidance_result.ret_state;
 245     }
 246
 247     // if timeout then path planner is taking too long to respond
 248     if (timed_out) {
 249         return OA_ERROR;
 250     }
 251
 252     // background thread is working on a new destination
 253     return OA_PROCESSING;
 254 }
 255
 256 // avoidance thread that continually updates the avoidance_result structure based on avoidance_request
 257 void AP_OAPathPlanner::avoidance_thread()
 258 {
 259     // require ekf origin to have been set
 260     bool origin_set = false;
 261     while (!origin_set) {
 262         hal.scheduler->delay(500);
 263         Location ekf_origin {};
 264         {
 265             WITH_SEMAPHORE(AP::ahrs().get_semaphore());
 266             origin_set = AP::ahrs().get_origin(ekf_origin);
 267         }
 268     }
 269
 270     while (true) {
 271
 272         // if database queue needs attention, service it faster
 273         if (_oadatabase.process_queue()) {
 274             hal.scheduler->delay(1);
 275         } else {
 276             hal.scheduler->delay(20);
 277         }
 278
 279         const uint32_t now = AP_HAL::millis();
 280         if (now - avoidance_latest_ms < OA_UPDATE_MS) {
 281             continue;
 282         }
 283         avoidance_latest_ms = now;
 284
 285         _oadatabase.update();
 286
 287         // values returned by path planners
 288         Location origin_new;
 289         Location destination_new;
 290         Location next_destination_new;
 291         bool dest_to_next_dest_clear = false;
 292         {
 293             WITH_SEMAPHORE(_rsem);
 294             if (now - avoidance_request.request_time_ms > OA_TIMEOUT_MS) {
 295                 // this is a very old request, don't process it
 296                 continue;
 297             }
 298
 299             // copy request to avoid conflict with main thread
 300             avoidance_request2 = avoidance_request;
 301
 302             // store passed in origin, destination and next_destination so we can return it if object avoidance is not required
 303             origin_new = avoidance_request.origin;
 304             destination_new = avoidance_request.destination;
 305             next_destination_new = avoidance_request.next_destination;
 306         }
 307
 308         // run background task looking for best alternative destination
 309         OA_RetState res = OA_NOT_REQUIRED;
 310         OAPathPlannerUsed path_planner_used = OAPathPlannerUsed::None;
 311         switch (_type) {
 312         case OA_PATHPLAN_DISABLED:
 313             continue;
 314         case OA_PATHPLAN_BENDYRULER: {
 315             if (_oabendyruler == nullptr) {
 316                 continue;
 317             }
 318             _oabendyruler->set_config(_margin_max);
 319
 320             AP_OABendyRuler::OABendyType bendy_type;
 321             if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, bendy_type, false)) {
 322                 res = OA_SUCCESS;
 323             }
 324             path_planner_used = map_bendytype_to_pathplannerused(bendy_type);
 325             break;
 326         }
 327
 328         case OA_PATHPLAN_DIJKSTRA: {
 329 #if AP_FENCE_ENABLED
 330             if (_oadijkstra == nullptr) {
 331                 continue;
 332             }
 333             _oadijkstra->set_fence_margin(_margin_max);
 334             const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc,
 335                                                                                           avoidance_request2.destination,
 336                                                                                           avoidance_request2.next_destination,
 337                                                                                           origin_new,
 338                                                                                           destination_new,
 339                                                                                           next_destination_new,
 340                                                                                           dest_to_next_dest_clear);
 341             switch (dijkstra_state) {
 342             case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
 343                 res = OA_NOT_REQUIRED;
 344                 break;
 345             case AP_OADijkstra::DIJKSTRA_STATE_ERROR:
 346                 res = OA_ERROR;
 347                 break;
 348             case AP_OADijkstra::DIJKSTRA_STATE_SUCCESS:
 349                 res = OA_SUCCESS;
 350                 break;
 351             }
 352             path_planner_used = OAPathPlannerUsed::Dijkstras;
 353 #endif
 354             break;
 355         }
 356
 357         case OA_PATHPLAN_DJIKSTRA_BENDYRULER: {
 358             if ((_oabendyruler == nullptr) || _oadijkstra == nullptr) {
 359                 continue;
 360             }
 361             _oabendyruler->set_config(_margin_max);
 362             AP_OABendyRuler::OABendyType bendy_type;
 363             if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, bendy_type, proximity_only)) {
 364                 // detected a obstacle by vehicle's proximity sensor. Switch avoidance to BendyRuler till obstacle is out of the way
 365                 proximity_only = false;
 366                 res = OA_SUCCESS;
 367                 path_planner_used = map_bendytype_to_pathplannerused(bendy_type);
 368                 break;
 369             } else {
 370                 // cleared all obstacles, trigger Dijkstra's to calculate path based on current deviated position
 371 #if AP_FENCE_ENABLED
 372                 if (proximity_only == false) {
 373                     _oadijkstra->recalculate_path();
 374                 }
 375 #endif
 376                 // only use proximity avoidance now for BendyRuler
 377                 proximity_only = true;
 378             }
 379 #if AP_FENCE_ENABLED
 380             _oadijkstra->set_fence_margin(_margin_max);
 381             const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc,
 382                                                                                           avoidance_request2.destination,
 383                                                                                           avoidance_request2.next_destination,
 384                                                                                           origin_new,
 385                                                                                           destination_new,
 386                                                                                           next_destination_new,
 387                                                                                           dest_to_next_dest_clear);
 388             switch (dijkstra_state) {
 389             case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
 390                 res = OA_NOT_REQUIRED;
 391                 break;
 392             case AP_OADijkstra::DIJKSTRA_STATE_ERROR:
 393                 res = OA_ERROR;
 394                 break;
 395             case AP_OADijkstra::DIJKSTRA_STATE_SUCCESS:
 396                 res = OA_SUCCESS;
 397                 break;
 398             }
 399             path_planner_used = OAPathPlannerUsed::Dijkstras;
 400 #endif
 401             break;
 402         }
 403
 404         } // switch
 405
 406         {
 407             // give the main thread the avoidance result
 408             WITH_SEMAPHORE(_rsem);
 409
 410             // place the destination and next destination used into the result (used by the caller to verify the result matches their request)
 411             avoidance_result.destination = avoidance_request2.destination;
 412             avoidance_result.next_destination = avoidance_request2.next_destination;
 413             avoidance_result.dest_to_next_dest_clear = dest_to_next_dest_clear;
 414
 415             // fill the result structure with the intermediate path
 416             avoidance_result.origin_new = (res == OA_SUCCESS) ? origin_new : avoidance_result.origin_new;
 417             avoidance_result.destination_new = (res == OA_SUCCESS) ? destination_new : avoidance_result.destination;
 418             avoidance_result.next_destination_new = (res == OA_SUCCESS) ? next_destination_new : avoidance_result.next_destination;
 419
 420             // create new avoidance result.dest_to_next_dest_clear field.  fill in with results from dijkstras or leave as unknown
 421             avoidance_result.result_time_ms = AP_HAL::millis();
 422             avoidance_result.path_planner_used = path_planner_used;
 423             avoidance_result.ret_state = res;
 424         }
 425     }
 426 }
 427
 428 // singleton instance
 429 AP_OAPathPlanner *AP_OAPathPlanner::_singleton;
 430
 431 namespace AP {
 432
 433 AP_OAPathPlanner *ap_oapathplanner()
 434 {
 435     return AP_OAPathPlanner::get_singleton();
 436 }
 437
 438 }
 439
 440 #endif  // AP_OAPATHPLANNER_ENABLED