Repository: ros-navigation/navigation2 (C++)

Issue:
nav2_smac_planner: check goal validity even when tolerance is non zero
## Feature request
Currently, the goal validity is not checked if the tolerance is greater than [0.001](https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp#L136) (cell). The idea behind this constraint is that it makes no sense to evaluate the validity of the goal itself when the tolerance is non zero as any valid point within the tolerance circle could also be considered as a replacement goal.

As a consequence, the SmacPlanner can try to compute a path to a zone which is obviously lethal (deep in lethal zone), just because the tolerance is non zero. The consequences are wasted time and resources.

#### Feature description
Find a way to evaluate the validity of the "tolerance circle" rather than the goal itself when the tolerance is non negligible.

#### Implementation considerations
This might be achieved by looking if there is at least one non lethal costmap cell which intersects the tolerance circle.



Discussion: Makes sense - I will mark as an enhancement. Would you want to contribute this or should I throw it to the community? 
I rapidly implemented and validated a prototype for Iron, which we are currently using: 

[0001-nav2_smac_planner-evaluate-goal-surroundings-validit.patch](https://github.com/user-attachments/files/22660830/0001-nav2_smac_planner-evaluate-goal-surroundings-validit.patch)

The main addition is the function `isZoneValid` below:
```
template<typename NodeT>
bool AStarAlgorithm<NodeT>::isZoneValid(const Coordinates &center_coords, float radius)
{
  // Define the search boundaries as a square around the center point
  // and clamp the boundaries to the costmap dimensions.
  int min_x = std::max(0, static_cast<int>(std::floor(center_coords.x - radius)));
  int max_x = std::min(
    static_cast<int>(getSizeX() - 1), static_cast<int>(std::ceil(center_coords.x + radius)));
  int min_y = std::max(0, static_cast<int>(std::floor(center_coords.y - radius)));
  int max_y = std::min(
    static_cast<int>(getSizeY() - 1), static_cast<int>(std::ceil(center_coords.y + radius)));

  // Pre-calculate squared radius for efficient distance checking
  const float radius_sq = radius * radius;

  for (int mx = min_x; mx <= max_x; ++mx) {
    for (int my = min_y; my <= max_y; ++my) {
      // Check if the cell is within the circular radius
      const float dx = mx - center_coords.x;
      const float dy = my - center_coords.y;
      if ((dx * dx + dy * dy) > radius_sq) {
        continue;
      }

      // To check the cell's validity, we create a temporary node instance
      // and use its isNodeValid() method.
      float theta = 0.0;
      if constexpr (!std::is_same_v<NodeT, Node2D>) {
        theta = center_coords.theta;
      }

      unsigned int index = NodeT::getIndex(
        static_cast<unsigned int>(mx),
        static_cast<unsigned int>(my),
        theta);

      NodeT temp_node(index);

      if constexpr (!std::is_same_v<NodeT, Node2D>) {
        temp_node.setPose(Coordinates(mx, my, theta));
      }

      // If the node representing this cell is valid, we've found a suitable point.
      if (temp_node.isNodeValid(_traverse_unknown, _collision_checker)) {
        return true;
      }
    }
  }

  // If the loops complete, no valid cell was found within the radius
  return false;
}

template<typename NodeT>
bool AStarAlgorithm<NodeT>::areInputsValid()
{
  // Check if graph was filled in
  if (_graph.empty()) {
    throw std::runtime_error("Failed to compute path, no costmap given.");
  }

  // Check if points were filled in
  if (!_start || !_goal) {
    throw std::runtime_error("Failed to compute path, no valid start or goal given.");
  }

  // Check if ending point is valid
  if (!isZoneValid(_goal_coordinates, _tolerance))
  {
    throw nav2_core::GoalOccupied("Goal was in lethal cost");
  }

  // Check if starting point is valid
  if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) {
    throw nav2_core::StartOccupied("Start was in lethal cost");
  }

  return true;
}
```

It should be migrated to the main branch. Unfortunately, I don't have any more time allowance at the moment for this issue. 
Great! I'll mark for community help to finish it up 😄 
I'd like to take ths issue and give it a try!
Please!
Based on my understanding of the given code, I see that only the zone for the final goal point is checked.

But existing code tries to check validity for all goal points within the set in [`removeInvalidGoals`](https://github.com/ros-navigation/navigation2/blob/2a2a709d4ee5015a63b2b9453578cf1d8b52f1d5/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp#L124) function, so I think zones for all the other goal points should also be checked right?

I was also wondering if zone checking for goal points could be a `GoalManager` function rather than the planner algorithm function by modifying the `removeInvalidGoals`.

Edit: Oh, after seeing how goal is set in [AStar code](https://github.com/ros-navigation/navigation2/blob/f78bac53252e6cb69a595761dd948f6e300c1d74/nav2_smac_planner/src/a_star.cpp#L214-L298), I see they're all in the same position with just different orientations so is that why it's redundant to check for all the points in the set? 

I'm trying to understand just that part, otherwise I find everything else okay and will port and test.
I think start with just the goal pose and we can expand from there. That code will be the same, at worst we'd just call it in a for loop for each goal orientation :-) Just make sure the implementation is in its own function that takes a goal as an input field.
I just completed implementing it and slightly updated the unit test to validate the new implementation, everything passes... I'll create a pull request and please take a look there to see if anything could be changed!
Great!



Fix (patch):
diff --git a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp
index 317a67de93..30db573748 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp
@@ -17,6 +17,7 @@
 #ifndef NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_
 #define NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_
 
+#include <algorithm>
 #include <unordered_set>
 #include <vector>
 #include <functional>
@@ -112,6 +113,74 @@ class GoalManager
     }
   }
 
+  /**
+   * @brief Checks if zone within the radius of a node is feasible. Returns true if
+   *        there's at least one non-lethal cell within the node radius.
+   * @param node Input node.
+   * @param radius Search radius.
+   * @param collision_checker Collision checker to validate nearby nodes.
+   * @param traverse_unknown Flag whether traversal through unknown space is allowed.
+   * @return true
+   * @return false
+   */
+  bool isZoneValid(
+    const NodePtr node, const float & radius, GridCollisionChecker * collision_checker,
+    const bool & traverse_unknown) const
+  {
+    if (radius < 1) {
+      return false;
+    }
+
+    const auto size_x = collision_checker->getCostmap()->getSizeInCellsX();
+    const auto size_y = collision_checker->getCostmap()->getSizeInCellsY();
+
+    auto getIndexFromPoint = [&size_x] (const Coordinates & point) {
+        unsigned int index = 0;
+
+        const auto mx = static_cast<unsigned int>(point.x);
+        const auto my = static_cast<unsigned int>(point.y);
+
+        if constexpr (!std::is_same_v<NodeT, Node2D>) {
+          const auto angle = static_cast<unsigned int>(point.theta);
+          index = NodeT::getIndex(mx, my, angle);
+        } else {
+          index = NodeT::getIndex(mx, my, size_x);
+        }
+
+        return index;
+      };
+
+    const Coordinates & center_point = node->pose;
+    const float min_x = std::max(0.0f, std::floor(center_point.x - radius));
+    const float min_y = std::max(0.0f, std::floor(center_point.y - radius));
+    const float max_x =
+      std::min(static_cast<float>(size_x - 1), std::ceil(center_point.x + radius));
+    const float max_y =
+      std::min(static_cast<float>(size_y - 1), std::ceil(center_point.y + radius));
+    const float radius_sq = radius * radius;
+
+    Coordinates m;
+    for (m.x = min_x; m.x <= max_x; m.x += 1.0f) {
+      for (m.y = min_y; m.y <= max_y; m.y += 1.0f) {
+        const float dx = m.x - center_point.x;
+        const float dy = m.y - center_point.y;
+
+        if (dx * dx + dy * dy > radius_sq) {
+          continue;
+        }
+
+        NodeT current_node(getIndexFromPoint(m));
+        current_node.setPose(m);
+
+        if (current_node.isNodeValid(traverse_unknown, collision_checker)) {
+          return true;
+        }
+      }
+    }
+
+    return false;
+  }
+
   /**
    * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds.
    *
@@ -133,7 +202,7 @@ class GoalManager
     }
     for (unsigned int i = 0; i < _goals_state.size(); i++) {
       if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) ||
-        tolerance > 0.001)
+        isZoneValid(_goals_state[i].goal, tolerance, collision_checker, traverse_unknown))
       {
         _goals_state[i].is_valid = true;
         _goals_set.insert(_goals_state[i].goal);
diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
index 6d54e1696a..d4cd916dc3 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
@@ -225,7 +225,7 @@ class Node2D
   }
 
   /**
-   * @brief Get index
+   * @brief Get coordinates using index
    * @param Index Index of point
    * @return coordinates of point
    */