diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index 1b3f8abd56..403c007fb0 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -63,7 +63,9 @@ inline BT::NodeStatus PathLongerOnApproach::tick() getInput("length_factor", length_factor_); if (first_time_ == false) { - if (old_path_.poses.back() != new_path_.poses.back()) { + if (old_path_.poses.empty() || new_path_.poses.empty() || + old_path_.poses.back() != new_path_.poses.back()) + { first_time_ = true; } }