Feature request

Currently, the goal validity is not checked if the tolerance is greater than 0.001 (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.