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 e6637ce4a3..e9d34279a5 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -88,8 +88,8 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Check for a valid potential value at a given point in the world // - must call computePotential first // - currently unused - bool validPointPotential(const geometry_msgs::msg::Point & world_point); - bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); + // bool validPointPotential(const geometry_msgs::msg::Point & world_point); + // bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); // Compute the squared distance between two points inline double squared_distance( diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 29c97db634..d291494589 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -357,42 +357,42 @@ NavfnPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point) return planner_->potarr[index]; } -bool -NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point) -{ - return validPointPotential(world_point, tolerance_); -} - -bool -NavfnPlanner::validPointPotential( - const geometry_msgs::msg::Point & world_point, double tolerance) -{ - const double resolution = costmap_->getResolution(); - - geometry_msgs::msg::Point p = world_point; - double potential = getPointPotential(p); - if (potential < POT_HIGH) { - // world_point is reachable by itself - return true; - } else { - // world_point, is not reachable. Trying to find any - // reachable point within its tolerance region - p.y = world_point.y - tolerance; - while (p.y <= world_point.y + tolerance) { - p.x = world_point.x - tolerance; - while (p.x <= world_point.x + tolerance) { - potential = getPointPotential(p); - if (potential < POT_HIGH) { - return true; - } - p.x += resolution; - } - p.y += resolution; - } - } - - return false; -} +// bool +// NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point) +// { +// return validPointPotential(world_point, tolerance_); +// } + +// bool +// NavfnPlanner::validPointPotential( +// const geometry_msgs::msg::Point & world_point, double tolerance) +// { +// const double resolution = costmap_->getResolution(); + +// geometry_msgs::msg::Point p = world_point; +// double potential = getPointPotential(p); +// if (potential < POT_HIGH) { +// // world_point is reachable by itself +// return true; +// } else { +// // world_point, is not reachable. Trying to find any +// // reachable point within its tolerance region +// p.y = world_point.y - tolerance; +// while (p.y <= world_point.y + tolerance) { +// p.x = world_point.x - tolerance; +// while (p.x <= world_point.x + tolerance) { +// potential = getPointPotential(p); +// if (potential < POT_HIGH) { +// return true; +// } +// p.x += resolution; +// } +// p.y += resolution; +// } +// } + +// return false; +// } bool NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)