[Global Planner] Fix Alt Prior handling (#553)

* [Global Planner] Fix Alt Prior handling alt_prior_ is pre-defined & limited length std::vector (currently 25 elements are defined) Accessing out of index of this vector for path finding can cause path finder to fail, thus high altitude RTL is not available in current status. This would allow high altitude RTL to work in current master, tested in SITL. * Fixed code style Co-authored-by: Seunghwan Jo <[email protected]>
PX4 · Jan 23, 2020 · 5c470d6 · 5c470d6
1 parent af5b035
commit 5c470d6
Showing 1 changed file with 9 additions and 2 deletions.
diff --git a/global_planner/src/library/global_planner.cpp b/global_planner/src/library/global_planner.cpp
@@ -161,8 +161,15 @@ double GlobalPlanner::getSingleCellRisk(const Cell& cell) {
 }
 
 double GlobalPlanner::getAltPrior(const Cell& cell) {
-  // return alt_prior_[cell.zIndex()];
-  return alt_prior_[std::round(cell.zPos())];
+  int index = std::round(cell.zPos());
+
+  if (index > alt_prior_.size() - 1) {
+    return alt_prior_.back();
+  } else if (index < 0) {
+    return alt_prior_.front();
+  } else {
+    return alt_prior_.at(index);
+  }
 }
 
 bool GlobalPlanner::isOccupied(const Cell& cell) { return getSingleCellRisk(cell) > 0.5; }