diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 86e852ebc9e..aee07de14c9 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/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" diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 73c5d59377b..447d02cc449 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/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" diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 567791d7f48..1c14027428f 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -18,7 +18,6 @@ #include #include #include "nav2_bt_navigator/navigators/navigate_through_poses.hpp" -#include "nav2_core/navigator_exceptions.hpp" namespace nav2_bt_navigator { @@ -122,16 +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 = @@ -169,21 +166,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; diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 2ee89cece21..d5c7ed20455 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -17,7 +17,6 @@ #include #include #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp" -#include "nav2_core/navigator_exceptions.hpp" namespace nav2_bt_navigator { @@ -124,18 +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 = @@ -173,25 +170,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_; diff --git a/nav2_core/include/nav2_core/navigator_exceptions.hpp b/nav2_core/include/nav2_core/navigator_exceptions.hpp deleted file mode 100644 index 53565a0df30..00000000000 --- a/nav2_core/include/nav2_core/navigator_exceptions.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2022 Joshua Wallace -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_CORE__NAVIGATOR_EXCEPTIONS_HPP_ -#define NAV2_CORE__NAVIGATOR_EXCEPTIONS_HPP_ - -#include -#include - -namespace nav2_core -{ - -class NavigatorException : public std::runtime_error -{ -public: - explicit NavigatorException(const std::string & description) - : std::runtime_error(description) {} -}; - -class NavigatorPoseNotAvailable : public NavigatorException -{ -public: - explicit NavigatorPoseNotAvailable(const std::string & description) - : NavigatorException(description) {} -}; - -class NavigatorInvalidPath : public NavigatorException -{ -public: - explicit NavigatorInvalidPath(const std::string & description) - : NavigatorException(description) {} -}; - -} // namespace nav2_core - -#endif // NAV2_CORE__NAVIGATOR_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 6708778cc1e..92f8b36f57d 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/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 diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 7afea7c8e22..4f2e41e0d56 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/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