remove exceptions from navigator onLoop (ros-navigation#4341)

Signed-off-by: Mike Wake <[email protected]>

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp

@@ -22,7 +22,6 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_core/behavior_tree_navigator.hpp"
-#include "nav2_core/navigator_exceptions.hpp"
 #include "nav2_msgs/action/navigate_through_poses.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/robot_utils.hpp"

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp

@@ -22,7 +22,6 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_core/behavior_tree_navigator.hpp"
-#include "nav2_core/navigator_exceptions.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -18,7 +18,6 @@
 #include <memory>
 #include <limits>
 #include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
-#include "nav2_core/navigator_exceptions.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -122,17 +121,14 @@ NavigateThroughPosesNavigator::onLoop()
       feedback_utils_.global_frame, feedback_utils_.robot_frame,
       feedback_utils_.transform_tolerance))
   {
-    throw nav2_core::NavigatorPoseNotAvailable("Robot pose is not available.");
+    // Robot pose is not yet available, no feedback
+    return;
   }
 
-  try {
-    // Get current path points
-    nav_msgs::msg::Path 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 nav2_core::NavigatorInvalidPath("no valid path available");
-    }
-
+  // Get current path points
+  nav_msgs::msg::Path current_path;
+  res = blackboard->get(path_blackboard_id_, current_path);
+  if (res && current_path.poses.size() > 0u) {
     // Find the closest pose to current pose on global path
     auto find_closest_pose_idx =
       [&current_pose, &current_path]() {
@@ -169,21 +165,6 @@ NavigateThroughPosesNavigator::onLoop()
 
     feedback_msg->distance_remaining = distance_remaining;
     feedback_msg->estimated_time_remaining = estimated_time_remaining;
-  } catch (const nav2_core::NavigatorPoseNotAvailable & ex) {
-    current_error_code_ = ActionT::Result::POSE_NOT_AVAILABLE;
-    current_error_msg_ = ex.what();
-    RCLCPP_ERROR(logger_, current_error_msg_.c_str());
-    // Returning since no point attempting recovery or publishing
-    // feedback until robot pose is available.
-    return;
-  } catch (const nav2_core::NavigatorInvalidPath & ex) {
-    current_error_code_ = ActionT::Result::NO_VALID_PATH;
-    current_error_msg_ = ex.what();
-    // Ignore ??
-  } catch (const std::runtime_error & ex) {
-    current_error_code_ = ActionT::Result::UNKNOWN;
-    current_error_msg_ = ex.what();
-    // Ignore ??
   }
 
   int recovery_count = 0;

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -17,7 +17,6 @@
 #include <memory>
 #include <limits>
 #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
-#include "nav2_core/navigator_exceptions.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -124,19 +123,16 @@ NavigateToPoseNavigator::onLoop()
       feedback_utils_.global_frame, feedback_utils_.robot_frame,
       feedback_utils_.transform_tolerance))
   {
-    throw nav2_core::NavigatorPoseNotAvailable("Robot pose is not available.");
+    RCLCPP_ERROR(logger_, "Robot pose is not available.");
+    return;
   }
 
   auto blackboard = bt_action_server_->getBlackboard();
 
-  try {
-    // Get current path points
-    nav_msgs::msg::Path 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 nav2_core::NavigatorInvalidPath("no valid path available");
-    }
-
+  // Get current path points
+  nav_msgs::msg::Path current_path;
+  [[maybe_unused]] auto res = blackboard->get(path_blackboard_id_, current_path);
+  if (current_path.poses.size() > 0u) {
     // Find the closest pose to current pose on global path
     auto find_closest_pose_idx =
       [&current_pose, &current_path]() {
@@ -173,25 +169,10 @@ NavigateToPoseNavigator::onLoop()
 
     feedback_msg->distance_remaining = distance_remaining;
     feedback_msg->estimated_time_remaining = estimated_time_remaining;
-  } catch (const nav2_core::NavigatorPoseNotAvailable & ex) {
-    current_error_code_ = ActionT::Result::POSE_NOT_AVAILABLE;
-    current_error_msg_ = ex.what();
-    RCLCPP_ERROR(logger_, current_error_msg_.c_str());
-    // Returning since no point attempting recovery or publishing
-    // feedback until robot pose is available.
-    return;
-  } catch (const nav2_core::NavigatorInvalidPath & ex) {
-    current_error_code_ = ActionT::Result::NO_VALID_PATH;
-    current_error_msg_ = ex.what();
-    // Ignore ??
-  } catch (const std::runtime_error & ex) {
-    current_error_code_ = ActionT::Result::UNKNOWN;
-    current_error_msg_ = ex.what();
-    // Ignore ??
   }
 
   int recovery_count = 0;
-  [[maybe_unused]] auto res = blackboard->get("number_recoveries", recovery_count);
+  res = blackboard->get("number_recoveries", recovery_count);
   feedback_msg->number_of_recoveries = recovery_count;
   feedback_msg->current_pose = current_pose;
   feedback_msg->navigation_time = clock_->now() - start_time_;

nav2_msgs/action/NavigateThroughPoses.action

@@ -11,7 +11,6 @@ uint16 NONE=0
 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=901
 uint16 TF_ERROR=902
 uint16 POSE_NOT_AVAILABLE=912
-uint16 NO_VALID_PATH=913
 uint16 UNKNOWN=999
 
 uint16 error_code

nav2_msgs/action/NavigateToPose.action

@@ -10,7 +10,6 @@ uint16 NONE=0
 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=901
 uint16 TF_ERROR=902
 uint16 POSE_NOT_AVAILABLE=912
-uint16 NO_VALID_PATH=913
 uint16 UNKNOWN=999
 
 uint16 error_code