From 9e0e1f6b785d89cebd876dfbe1fbed7bea8c644f Mon Sep 17 00:00:00 2001 From: Kemal Bektas Date: Mon, 26 Feb 2024 14:40:33 +0100 Subject: [PATCH] Support cancel in Navfn planner Signed-off-by: Kemal Bektas --- .../include/nav2_navfn_planner/navfn.hpp | 18 +++++++++++----- .../nav2_navfn_planner/navfn_planner.hpp | 6 +++++- nav2_navfn_planner/src/navfn.cpp | 21 +++++++++++++------ nav2_navfn_planner/src/navfn_planner.cpp | 10 +++++---- 4 files changed, 39 insertions(+), 16 deletions(-) diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index a4ce84404a8..6860f41de6b 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace nav2_navfn_planner { @@ -131,14 +132,16 @@ class NavFn /** * @brief Calculates a plan using the A* heuristic, returns true if one is found + * @param cancelChecker Function to check if the task has been canceled * @return True if a plan is found, false otherwise */ - bool calcNavFnAstar(); + bool calcNavFnAstar(std::function cancelChecker); /** - * @brief Caclulates the full navigation function using Dijkstra + * @brief Calculates the full navigation function using Dijkstra + * @param cancelChecker Function to check if the task has been canceled */ - bool calcNavFnDijkstra(bool atStart = false); + bool calcNavFnDijkstra(std::function cancelChecker, bool atStart = false); /** * @brief Accessor for the x-coordinates of a path @@ -179,6 +182,9 @@ class NavFn float curT; /**< current threshold */ float priInc; /**< priority threshold increment */ + /**< number of cycles between checks for cancellation */ + int cancel_check_interval; + /** goal and start positions */ /** * @brief Sets the goal position for the planner. @@ -229,18 +235,20 @@ class NavFn * @brief Run propagation for iterations, or until start is reached using * breadth-first Dijkstra method * @param cycles The maximum number of iterations to run for + * @param cancelChecker Function to check if the task has been canceled * @param atStart Whether or not to stop when the start point is reached * @return true if the start point is reached */ - bool propNavFnDijkstra(int cycles, bool atStart = false); + bool propNavFnDijkstra(int cycles, std::function cancelChecker, bool atStart = false); /** * @brief Run propagation for iterations, or until start is reached using * the best-first A* method with Euclidean distance heuristic * @param cycles The maximum number of iterations to run for + * @param cancelChecker Function to check if the task has been canceled * @return true if the start point is reached */ - bool propNavFnAstar(int cycles); /**< returns true if start point found */ + bool propNavFnAstar(int cycles, std::function cancelChecker); /** gradient and paths */ float * gradx, * grady; /**< gradient arrays, size of potential array */ diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 2330a5165b9..c1cebb1bef0 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -81,11 +81,13 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the task has been canceled * @return nav_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -93,12 +95,14 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @param start Start pose * @param goal Goal pose * @param tolerance Relaxation constraint in x and y + * @param cancel_checker Function to check if the task has been canceled * @param plan Path to be computed * @return true if can find the path */ bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, + std::function cancel_checker, nav_msgs::msg::Path & plan); /** diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 8259950b038..cb1dd4054c2 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -44,6 +44,7 @@ #include "nav2_navfn_planner/navfn.hpp" #include +#include "nav2_core/planner_exceptions.hpp" #include "rclcpp/rclcpp.hpp" namespace nav2_navfn_planner @@ -293,12 +294,12 @@ NavFn::setCostmap(const COSTTYPE * cmap, bool isROS, bool allow_unknown) } bool -NavFn::calcNavFnDijkstra(bool atStart) +NavFn::calcNavFnDijkstra(std::function cancelChecker, bool atStart) { setupNavFn(true); // calculate the nav fn and path - return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), atStart); + return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), cancelChecker, atStart); } @@ -307,12 +308,12 @@ NavFn::calcNavFnDijkstra(bool atStart) // bool -NavFn::calcNavFnAstar() +NavFn::calcNavFnAstar(std::function cancelChecker) { setupNavFn(true); // calculate the nav fn and path - return propNavFnAstar(std::max(nx * ny / 20, nx + ny)); + return propNavFnAstar(std::max(nx * ny / 20, nx + ny), cancelChecker); } // @@ -571,7 +572,7 @@ NavFn::updateCellAstar(int n) // bool -NavFn::propNavFnDijkstra(int cycles, bool atStart) +NavFn::propNavFnDijkstra(int cycles, std::function cancelChecker, bool atStart) { int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks @@ -581,6 +582,10 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) int startCell = start[1] * nx + start[0]; for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted + if (cycle % cancel_check_interval == 0 && cancelChecker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } + if (curPe == 0 && nextPe == 0) { // priority blocks empty break; } @@ -652,7 +657,7 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) // bool -NavFn::propNavFnAstar(int cycles) +NavFn::propNavFnAstar(int cycles, std::function cancelChecker) { int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks @@ -667,6 +672,10 @@ NavFn::propNavFnAstar(int cycles) // do main cycle for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted + if (cycle % cancel_check_interval == 0 && cancelChecker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } + if (curPe == 0 && nextPe == 0) { // priority blocks empty break; } diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ee8d69f53e..9985280159e 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -129,7 +129,8 @@ NavfnPlanner::cleanup() nav_msgs::msg::Path NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { #ifdef BENCHMARK_TESTING steady_clock::time_point a = steady_clock::now(); @@ -183,7 +184,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( return path; } - if (!makePlan(start.pose, goal.pose, tolerance_, path)) { + if (!makePlan(start.pose, goal.pose, tolerance_, cancel_checker, path)) { throw nav2_core::NoValidPathCouldBeFound( "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } @@ -214,6 +215,7 @@ bool NavfnPlanner::makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, + std::function cancel_checker, nav_msgs::msg::Path & plan) { // clear the plan, just in case @@ -261,9 +263,9 @@ NavfnPlanner::makePlan( planner_->setStart(map_goal); planner_->setGoal(map_start); if (use_astar_) { - planner_->calcNavFnAstar(); + planner_->calcNavFnAstar(cancel_checker); } else { - planner_->calcNavFnDijkstra(true); + planner_->calcNavFnDijkstra(cancel_checker, true); } double resolution = costmap_->getResolution();