feat: add DistanceMap (#1142)

* feat: add DistanceMap

* feat: add DistanceMap test

* feat: add DistanceMap doc

* feat: DistanceMap doc update

* feat: DistanceMap update

docs/modules/3_mapping/distance_map/distance_map_main.rst

@@ -0,0 +1,27 @@
+Distance Map
+------------
+
+This is an implementation of the Distance Map algorithm for path planning.
+
+The Distance Map algorithm computes the unsigned distance field (UDF) and signed distance field (SDF) from a boolean field representing obstacles. 
+
+The UDF gives the distance from each point to the nearest obstacle. The SDF gives positive distances for points outside obstacles and negative distances for points inside obstacles.
+
+Example
+~~~~~~~
+
+The algorithm is demonstrated on a simple 2D grid with obstacles:
+
+.. image:: distance_map.png
+
+API
+~~~
+
+.. autofunction:: PathPlanning.DistanceMap.distance_map.compute_sdf
+
+.. autofunction:: PathPlanning.DistanceMap.distance_map.compute_udf
+
+References
+~~~~~~~~~~
+
+- `Distance Transforms of Sampled Functions <https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf>`_ paper by Pedro F. Felzenszwalb and Daniel P. Huttenlocher. 

docs/modules/3_mapping/mapping_main.rst

@@ -17,3 +17,4 @@ Mapping is the ability of a robot to understand its surroundings with external s
    circle_fitting/circle_fitting
    rectangle_fitting/rectangle_fitting
    normal_vector_estimation/normal_vector_estimation
+   distance_map/distance_map