#0  __memmove_avx_unaligned_erms () at ../sysdeps/x86_64/multiarch/memmove-vec-unaligned-erms.S:306
#1  0x00007ffff7b2e322 in std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >::_M_construct<char*> (this=0x7fff695f0a18, 
    __beg=0x100000001 <error: Cannot access memory at address 0x100000001>, 
    __end=0x100000005 <error: Cannot access memory at address 0x100000005>)
    at /usr/include/c++/7/bits/basic_string.tcc:225
#2  0x00007ffff7b18a59 in std_msgs::msg::Header_<std::allocator<void> >::Header_ (
    this=0x7fff695f0a10) at /opt/ros/eloquent/include/std_msgs/msg/header__struct.hpp:35
#3  0x00007ffff7b18abf in geometry_msgs::msg::PoseStamped_<std::allocator<void> >::PoseStamped_ (
    this=0x7fff695f0a10) at /opt/ros/eloquent/include/geometry_msgs/msg/pose_stamped__struct.hpp:37
#4  0x00007fff801b6ea9 in nav2_navfn_planner::NavfnPlanner::smoothApproachToGoal (
    this=0x5555565cd870, goal=..., plan=...)
    at ros2_ws/src/navigation2/nav2_navfn_planner/src/navfn_planner.cpp:250

diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index 6b8ccafd..daa642e7 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -246,7 +246,9 @@ 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.
-
+  if (plan.poses.size() < 2){
+    return;
+  }
   auto second_to_last_pose = plan.poses.end()[-2];
   auto last_pose = plan.poses.back();
   if (