Force potential seeking algorithms to step by cells

nav2_navfn_planner/src/navfn_planner.cpp

@@ -218,11 +218,16 @@ NavfnPlanner::makePlan(
   bool found_legal = false;
   double best_sdist = std::numeric_limits<double>::max();
 
-  p.position.y = goal.position.y - tolerance;
+  // Make resolution_tolerance to be multiple of resolution, but not exceed the tolerance.
+  // This will avoid the situations when p could take its values between cells
+  // which leads to accuracy lost.
+  const double resolution_tolerance = std::trunc(tolerance / resolution) * resolution;
 
-  while (p.position.y <= goal.position.y + tolerance) {
-    p.position.x = goal.position.x - tolerance;
-    while (p.position.x <= goal.position.x + tolerance) {
+  p.position.y = goal.position.y - resolution_tolerance;
+
+  while (p.position.y <= goal.position.y + resolution_tolerance) {
+    p.position.x = goal.position.x - resolution_tolerance;
+    while (p.position.x <= goal.position.x + resolution_tolerance) {
       double potential = getPointPotential(p.position);
       double sdist = squared_distance(p, goal);
       if (potential < POT_HIGH && sdist < best_sdist) {
@@ -360,12 +365,17 @@ NavfnPlanner::validPointPotential(
 {
   const double resolution = costmap_->getResolution();
 
+  // Make resolution_tolerance to be multiple of resolution, but not exceed the tolerance.
+  // This will avoid the situations when p could take its values between cells
+  // which leads to accuracy lost.
+  const double resolution_tolerance = std::trunc(tolerance / resolution) * resolution;
+
   geometry_msgs::msg::Point p = world_point;
-  p.y = world_point.y - tolerance;
+  p.y = world_point.y - resolution_tolerance;
 
-  while (p.y <= world_point.y + tolerance) {
-    p.x = world_point.x - tolerance;
-    while (p.x <= world_point.x + tolerance) {
+  while (p.y <= world_point.y + resolution_tolerance) {
+    p.x = world_point.x - resolution_tolerance;
+    while (p.x <= world_point.x + resolution_tolerance) {
       double potential = getPointPotential(p);
       if (potential < POT_HIGH) {
         return true;