Feature/costmap integration

Many changes related to my work on mobile manipulation.

The collision checking is now done initially during onConnectComponent. A costmap will be created and global planning is now based on this. Global planning now only takes ~1ms. However, the creation of the costmap might take several seconds (~10-20s).

Instead of A*, users can now also select the ShortestPathFasterAlgorithm which is applicable if the graph has non-positive edge weights. This might be the case if the distance to the obstacles is considered. The resulting trajectory:

source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp

@@ -22,6 +22,7 @@
 #include "ShortestPathFasterAlgorithm.h"
 
 #include <algorithm>
+#include <numeric>
 
 #include <Eigen/Core>
 
@@ -128,6 +129,8 @@ namespace armarx::navigation::algorithms::spfa
     ShortestPathFasterAlgorithm::spfa(const Eigen::MatrixXf& inputMap,
                                       const Eigen::Vector2i& source) const
     {
+        ARMARX_CHECK_GREATER(inputMap(source.x(), source.y()), 0.F) << "Start must not be in collision";
+
         const float eps = 1e-6;
         const int num_dirs = 8;
 
@@ -160,9 +163,8 @@ namespace armarx::navigation::algorithms::spfa
         float* dists = new float[max_num_verts];
         for (int i = 0; i < max_num_verts; ++i)
             dists[i] = inf;
-        int* parents = new int[max_num_verts]();
-        for (int i = 0; i < max_num_verts; ++i)
-            parents[i] = -1;
+
+        std::vector<int> parents(max_num_verts, -1);
 
         // Build graph
         ARMARX_INFO << "Build graph";
@@ -249,6 +251,8 @@ namespace armarx::navigation::algorithms::spfa
                                                                   Eigen::Vector2i{-1, -1}));
         }
 
+        int invalids = 0;
+
         for (int row = 0; row < num_rows; ++row)
         {
             for (int col = 0; col < num_cols; ++col)
@@ -258,6 +262,7 @@ namespace armarx::navigation::algorithms::spfa
 
                 if (parents[u] == -1) // no parent
                 {
+                    invalids++;
                     continue;
                 }
 
@@ -265,6 +270,10 @@ namespace armarx::navigation::algorithms::spfa
             }
         }
 
+
+        ARMARX_INFO << "Fraction of invalid cells: (" << invalids << "/" << parents.size() << ")";
+
+
         // Free memory
         delete[] edges;
         delete[] edge_counts;
@@ -272,7 +281,6 @@ namespace armarx::navigation::algorithms::spfa
         delete[] in_queue;
         delete[] weights;
         delete[] dists;
-        delete[] parents;
 
         ARMARX_INFO << "Done.";
         return Result{.distances = output_dists, .parents = output_parents};