Prevent a possible segmentation fault (ros-navigation#4141) (ros-navi…

…gation#4147) * Prevent a possible segmentation fault ros-navigation#4141 Signed-off-by: Joni Pöllänen <[email protected]> * Cleanup Signed-off-by: Joni Pöllänen <[email protected]> --------- Signed-off-by: Joni Pöllänen <[email protected]> (cherry picked from commit 654e7e0)
logivations · Nov 11, 2024 · d31df51 · d31df51
1 parent ef91d08
commit d31df51
Showing 1 changed file with 3 additions and 4 deletions.
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -186,12 +186,10 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   // into higher cost areas far out from the goal itself, let search to the work of getting
   // close before the analytic expansion brings it home. This should never be smaller than
   // 4-5x the minimum turning radius being used, or planning times will begin to spike.
-  if (d > _search_info.analytic_expansion_max_length) {
+  if (d > _search_info.analytic_expansion_max_length || d < sqrt_2) {
     return AnalyticExpansionNodes();
   }
 
-  // A move of sqrt(2) is guaranteed to be in a new cell
-  static const float sqrt_2 = std::sqrt(2.0f);
   unsigned int num_intervals = static_cast<unsigned int>(std::floor(d / sqrt_2));
 
   AnalyticExpansionNodes possible_nodes;
@@ -251,7 +249,8 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   if (!failure) {
     // We found 'a' valid expansion. Now to tell if its a quality option...
     const float max_cost = _search_info.analytic_expansion_max_cost;
-    if (*std::max_element(node_costs.begin(), node_costs.end()) > max_cost) {
+    auto max_cost_it = std::max_element(node_costs.begin(), node_costs.end());
+    if (max_cost_it != node_costs.end() && *max_cost_it > max_cost) {
       // If any element is above the comfortable cost limit, check edge cases:
       // (1) Check if goal is in greater than max_cost space requiring
       //  entering it, but only entering it on final approach, not in-and-out