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/Costmap.cpp

@@ -62,5 +62,38 @@ namespace armarx::navigation::algorithms
         return grid(v.index.x(), v.index.y()) == 0.F;
     }
 
+    bool
+    Costmap::add(const Costmap& other)
+    {
+        const auto startIdx = toVertex(other.sceneBounds.min);
+
+        // merge other grid into this one => use max costs
+
+        // TODO if one of both matrices has mask, it has to be considered.
+        ARMARX_TRACE;
+        grid.block(startIdx.index.x(), startIdx.index.y(), other.grid.rows(), other.grid.cols()) =
+            grid.block(startIdx.index.x(), startIdx.index.y(), other.grid.rows(), other.grid.cols())
+                .cwiseMax(other.grid);
+
+        if(other.mask.has_value())
+        {
+            if(not mask.has_value())
+            {
+                mask = Mask(grid.rows(), grid.cols());
+                mask->setOnes();
+            }
+
+            // union of masks
+            ARMARX_TRACE;
+            mask = mask->cwiseMin(other.mask->value());
+
+            // apply mask to grid => all invalid points will be 0
+            ARMARX_TRACE;
+            grid = grid.cwiseProduct(mask->cast<float>());
+        }
+
+        return true;
+    }
+
 
 } // namespace armarx::navigation::algorithms