Issue 1608 segmentation fault planner server node master (#1612) (#1767)

* corrected wrong indexing in NavfnPlanner::smoothApproachToGoal function

* resolved uncrustify errors

* corrected the condition inside NavfnPlanner::smoothApproachToGoal to replace last pose of computed path

Co-authored-by: chikmagalore.thilak <chikmagalore.thilak@bshg.com>

Co-authored-by: crthilakraj <crthilakraj@users.noreply.github.com>
Co-authored-by: chikmagalore.thilak <chikmagalore.thilak@bshg.com>

Author: 3 people

nav2_navfn_planner/src/navfn_planner.cpp

@@ -246,19 +246,20 @@ NavfnPlanner::smoothApproachToGoal(
 {
   // Replace the last pose of the computed path if it's actually further away
   // to the second to last pose than the goal pose.
-
-  auto second_to_last_pose = plan.poses.end()[-2];
-  auto last_pose = plan.poses.back();
-  if (
-    squared_distance(last_pose.pose, second_to_last_pose.pose) >
-    squared_distance(goal, second_to_last_pose.pose))
-  {
-    plan.poses.back().pose = goal;
-  } else {
-    geometry_msgs::msg::PoseStamped goal_copy;
-    goal_copy.pose = goal;
-    plan.poses.push_back(goal_copy);
+  if (plan.poses.size() >= 2) {
+    auto second_to_last_pose = plan.poses.end()[-2];
+    auto last_pose = plan.poses.back();
+    if (
+      squared_distance(last_pose.pose, second_to_last_pose.pose) >
+      squared_distance(goal, second_to_last_pose.pose))
+    {
+      plan.poses.back().pose = goal;
+      return;
+    }
   }
+  geometry_msgs::msg::PoseStamped goal_copy;
+  goal_copy.pose = goal;
+  plan.poses.push_back(goal_copy);
 }
 
 bool