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

* Improving stability of feedback * Adding Nsv through poses
quasi-robotics · Oct 22, 2024 · 96f546c · 96f546c
1 parent 81ffb4e
commit 96f546c
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 2 deletions.
diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/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 =

diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/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 =