From 17b12ec2d6bfe1721fa6910053f55aefc645dd2b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joni=20P=C3=B6ll=C3=A4nen?=
 <jonipol@users.noreply.github.com>
Date: Mon, 26 Feb 2024 15:30:41 +0200
Subject: [PATCH 1/2] Prevent a possible segmentation fault
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

#4141
Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
---
 nav2_smac_planner/src/analytic_expansion.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index 7c73fb91da7..93026f64aa3 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -189,6 +189,10 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
 
   // A move of sqrt(2) is guaranteed to be in a new cell
   static const float sqrt_2 = std::sqrt(2.0f);
+  if (d < sqrt_2) {
+    return AnalyticExpansionNodes();
+  }
+
   unsigned int num_intervals = static_cast<unsigned int>(std::floor(d / sqrt_2));
 
   AnalyticExpansionNodes possible_nodes;
@@ -248,7 +252,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

From 42393d38c0d4107217bd6e04988755bb54595913 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joni=20P=C3=B6ll=C3=A4nen?= <joni.pollanen@karelics.fi>
Date: Tue, 27 Feb 2024 12:49:24 +0200
Subject: [PATCH 2/2] Cleanup
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
---
 nav2_smac_planner/src/analytic_expansion.cpp | 11 ++++-------
 1 file changed, 4 insertions(+), 7 deletions(-)

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index 93026f64aa3..8dcc1314357 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -179,17 +179,14 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
 
   float d = state_space->distance(from(), to());
 
+  // A move of sqrt(2) is guaranteed to be in a new cell
+  static const float sqrt_2 = std::sqrt(2.0f);
+
   // If the length is too far, exit. This prevents unsafe shortcutting of paths
   // 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) {
-    return AnalyticExpansionNodes();
-  }
-
-  // A move of sqrt(2) is guaranteed to be in a new cell
-  static const float sqrt_2 = std::sqrt(2.0f);
-  if (d < sqrt_2) {
+  if (d > _search_info.analytic_expansion_max_length || d < sqrt_2) {
     return AnalyticExpansionNodes();
   }