From 27ed8e9f13815a40ce92f53c2e73d362742b4d81 Mon Sep 17 00:00:00 2001 From: Vince Reda <60265874+redvinaa@users.noreply.github.com> Date: Wed, 7 Aug 2024 18:00:17 +0200 Subject: [PATCH] Copy fix-terminate diff from opennav_docking repo (#4598) * Copy fix-terminate diff from opennav_docking repo Signed-off-by: redvinaa * Lint Signed-off-by: redvinaa --------- Signed-off-by: redvinaa --- .../opennav_docking/docking_server.hpp | 1 + .../include/opennav_docking/navigator.hpp | 8 +++- .../opennav_docking/src/docking_server.cpp | 9 +++- .../opennav_docking/src/navigator.cpp | 43 +++++++++++++++---- .../opennav_docking/test/test_navigator.cpp | 24 +++++++++-- 5 files changed, 69 insertions(+), 16 deletions(-) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index bab26bef5a..08c3cc1f7c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp b/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp index 341ad936dd..f12b92883e 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -67,11 +68,14 @@ class Navigator * May throw exception if fails to navigate or communicate * Blocks until completion. * @param pose Pose to go to - * @param max_staging_duration Maximum time to get to the staging pose + * @param remaining_staging_duration Remaining time to get to the staging pose + * @param isPreempted Function to check if preempted + * @param recursed True if recursed (used to retry once) */ void goToPose( const geometry_msgs::msg::PoseStamped & pose, - const rclcpp::Duration & max_staging_duration, + rclcpp::Duration remaining_staging_duration, + std::function isPreempted, bool recursed = false); protected: diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index e1b7c19279..a8bf62a6b2 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -253,8 +253,15 @@ void DockingServer::dockRobot() { RCLCPP_INFO(get_logger(), "Robot already within pre-staging pose tolerance for dock"); } else { + std::function isPreempted = [this]() { + return checkAndWarnIfCancelled(docking_action_server_, "dock_robot") || + checkAndWarnIfPreempted(docking_action_server_, "dock_robot"); + }; + navigator_->goToPose( - initial_staging_pose, rclcpp::Duration::from_seconds(goal->max_staging_time)); + initial_staging_pose, + rclcpp::Duration::from_seconds(goal->max_staging_time), + isPreempted); RCLCPP_INFO(get_logger(), "Successful navigation to staging pose"); } diff --git a/nav2_docking/opennav_docking/src/navigator.cpp b/nav2_docking/opennav_docking/src/navigator.cpp index aae3ff9709..e792411a75 100644 --- a/nav2_docking/opennav_docking/src/navigator.cpp +++ b/nav2_docking/opennav_docking/src/navigator.cpp @@ -51,13 +51,16 @@ void Navigator::deactivate() void Navigator::goToPose( const geometry_msgs::msg::PoseStamped & pose, - const rclcpp::Duration & max_staging_duration, + rclcpp::Duration remaining_staging_duration, + std::function isPreempted, bool recursed) { + auto node = node_.lock(); + Nav2Pose::Goal goal; goal.pose = pose; goal.behavior_tree = navigator_bt_xml_; - const auto timeout = max_staging_duration.to_chrono(); + const auto start_time = node->now(); // Wait for server to be active nav_to_pose_client_->wait_for_action_server(1s); @@ -66,19 +69,41 @@ void Navigator::goToPose( future_goal_handle, 2s) == rclcpp::FutureReturnCode::SUCCESS) { auto future_result = nav_to_pose_client_->async_get_result(future_goal_handle.get()); - if (executor_.spin_until_future_complete( - future_result, timeout) == rclcpp::FutureReturnCode::SUCCESS) - { - auto result = future_result.get(); - if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->error_code == 0) { - return; // Success! + + while (rclcpp::ok()) { + if (isPreempted()) { + auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get()); + executor_.spin_until_future_complete(cancel_future, 1s); + throw opennav_docking_core::FailedToStage("Navigation request to staging pose preempted."); + } + + if (node->now() - start_time > remaining_staging_duration) { + auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get()); + executor_.spin_until_future_complete(cancel_future, 1s); + throw opennav_docking_core::FailedToStage("Navigation request to staging pose timed out."); + } + + if (executor_.spin_until_future_complete( + future_result, 10ms) == rclcpp::FutureReturnCode::SUCCESS) + { + auto result = future_result.get(); + if (result.code == rclcpp_action::ResultCode::SUCCEEDED && + result.result->error_code == 0) + { + return; // Success! + } else { + RCLCPP_WARN(node->get_logger(), "Navigation request to staging pose failed."); + break; + } } } } // Attempt to retry once using single iteration recursion if (!recursed) { - goToPose(pose, max_staging_duration, true); + auto elapsed_time = node->now() - start_time; + remaining_staging_duration = remaining_staging_duration - elapsed_time; + goToPose(pose, remaining_staging_duration, isPreempted, true); return; } diff --git a/nav2_docking/opennav_docking/test/test_navigator.cpp b/nav2_docking/opennav_docking/test/test_navigator.cpp index 80766f0782..8d81dec81b 100644 --- a/nav2_docking/opennav_docking/test/test_navigator.cpp +++ b/nav2_docking/opennav_docking/test/test_navigator.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_util/simple_action_server.hpp" @@ -94,26 +96,40 @@ TEST(NavigatorTests, TestNavigator) auto navigator = std::make_unique(node); navigator->activate(); + std::function is_preempted_true = []() {return true;}; + std::function is_preempted_false = []() {return false;}; + // Should succeed, action server set to succeed dummy_navigator_node->setReturn(true); EXPECT_NO_THROW( - navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0))); + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), + is_preempted_false)); // Should fail, timeout exceeded EXPECT_THROW( - navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0)), + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0), + is_preempted_false), opennav_docking_core::FailedToStage); // Should fail, action server set to succeed dummy_navigator_node->setReturn(false); EXPECT_THROW( - navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)), + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), + is_preempted_false), opennav_docking_core::FailedToStage); // First should fail, recursion should succeed dummy_navigator_node->setToggle(); EXPECT_NO_THROW( - navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0))); + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), + is_preempted_false)); + + // Should fail, preempted + dummy_navigator_node->setReturn(true); + EXPECT_THROW( + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), + is_preempted_true), + opennav_docking_core::FailedToStage); navigator->deactivate(); navigator.reset();