Improving stability of action feedback (ros-navigation#4225)

* Improving stability of feedback

* Adding Nsv through poses

Signed-off-by: stevedanomodolor <[email protected]>

Author: SteveMacenski

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -123,7 +123,10 @@ NavigateThroughPosesNavigator::onLoop()
   try {
     // Get current path points
     nav_msgs::msg::Path current_path;
-    res = blackboard->get(path_blackboard_id_, current_path);
+    if (!blackboard->get(path_blackboard_id_, current_path) || current_path.poses.size() == 0u) {
+      // If no path set yet or not meaningful, can't compute ETA or dist remaining yet.
+      throw std::exception();
+    }
 
     // Find the closest pose to current pose on global path
     auto find_closest_pose_idx =

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -126,7 +126,10 @@ NavigateToPoseNavigator::onLoop()
   try {
     // Get current path points
     nav_msgs::msg::Path current_path;
-    [[maybe_unused]] auto res = blackboard->get(path_blackboard_id_, current_path);
+    if (!blackboard->get(path_blackboard_id_, current_path) || current_path.poses.size() == 0u) {
+      // If no path set yet or not meaningful, can't compute ETA or dist remaining yet.
+      throw std::exception();
+    }
 
     // Find the closest pose to current pose on global path
     auto find_closest_pose_idx =