From a7875b687266a9425a96b8bfed06f1535d896e8a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 27 Mar 2024 18:10:56 -0700 Subject: [PATCH 1/2] Improving stability of feedback --- nav2_bt_navigator/src/navigators/navigate_to_pose.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 2270b6add5..be6a294835 100644 --- 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 = From dffd37c463e314361e4c3c9b162924bf17d50595 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 27 Mar 2024 18:12:30 -0700 Subject: [PATCH 2/2] Adding Nsv through poses --- nav2_bt_navigator/src/navigators/navigate_through_poses.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index a4b4df52d1..98d8649980 100644 --- 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 =