making collision checking for start optional (restoring old behavior)

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

@@ -29,9 +29,10 @@
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
-#include "range/v3/algorithm/reverse.hpp"
 #include <armarx/navigation/algorithms/Costmap.h>
 
+#include "range/v3/algorithm/reverse.hpp"
+
 namespace armarx::navigation::algorithms::spfa
 {
 
@@ -64,7 +65,8 @@ namespace armarx::navigation::algorithms::spfa
 
     ShortestPathFasterAlgorithm::PlanningResult
     ShortestPathFasterAlgorithm::plan(const Eigen::Vector2f& start,
-                                      const Eigen::Vector2f& goal) const
+                                      const Eigen::Vector2f& goal,
+                                      const bool checkStartForCollision) const
     {
         // ARMARX_CHECK(not costmap.isInCollision(start)) << "Start is in collision";
         // ARMARX_CHECK(not costmap.isInCollision(goal)) << "Goal is in collision";
@@ -81,12 +83,13 @@ namespace armarx::navigation::algorithms::spfa
         }
 
         auto costmapWithValidStart = costmap.getGrid();
-        if(costmap.isInCollision(start))
+        if (costmap.isInCollision(start))
         {
             costmapWithValidStart(startVertex.index.x(), startVertex.index.y()) = 0.1;
         }
 
-        const Result result = spfa(costmapWithValidStart, Eigen::Vector2i{startVertex.index});
+        const Result result =
+            spfa(costmapWithValidStart, Eigen::Vector2i{startVertex.index}, checkStartForCollision);
 
         const auto isStart = [&startVertex](const Eigen::Vector2i& v) -> bool
         { return (v.array() == startVertex.index.array()).all(); };
@@ -133,18 +136,22 @@ namespace armarx::navigation::algorithms::spfa
     }
 
     ShortestPathFasterAlgorithm::Result
-    ShortestPathFasterAlgorithm::spfa(const Eigen::Vector2f& start) const
+    ShortestPathFasterAlgorithm::spfa(const Eigen::Vector2f& start,
+                                      const bool checkStartForCollision) const
     {
-        return spfa(costmap.getGrid(), costmap.toVertex(start).index);
+        return spfa(costmap.getGrid(), costmap.toVertex(start).index, checkStartForCollision);
     }
 
-
     ShortestPathFasterAlgorithm::Result
     ShortestPathFasterAlgorithm::spfa(const Eigen::MatrixXf& inputMap,
-                                      const Eigen::Vector2i& source) const
+                                      const Eigen::Vector2i& source,
+                                      const bool checkStartForCollision) const
     {
-        ARMARX_CHECK_GREATER(inputMap(source.x(), source.y()), 0.F)
-            << "Start must not be in collision";
+        if (checkStartForCollision)
+        {
+            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;

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

@@ -27,7 +27,6 @@
 
 #include <armarx/navigation/algorithms/Costmap.h>
 
-
 namespace armarx::navigation::algorithms::spfa
 {
 
@@ -39,7 +38,7 @@ namespace armarx::navigation::algorithms::spfa
             bool obstacleDistanceCosts = true;
             float obstacleMaxDistance = 1000.F;
 
-            float obstacleDistanceWeight = 1.3F; 
+            float obstacleDistanceWeight = 1.3F;
 
             float obstacleCostExponent = 4.F;
         };
@@ -69,10 +68,11 @@ namespace armarx::navigation::algorithms::spfa
             }
         };
 
-        PlanningResult plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal) const;
-
+        PlanningResult plan(const Eigen::Vector2f& start,
+                            const Eigen::Vector2f& goal,
+                            bool checkStartForCollision = true) const;
 
-        Result spfa(const Eigen::Vector2f& start) const;
+        Result spfa(const Eigen::Vector2f& start, bool checkStartForCollision = true) const;
 
         // protected:
         /**
@@ -82,7 +82,9 @@ namespace armarx::navigation::algorithms::spfa
          * @param source 
          * @return Result 
          */
-        Result spfa(const Eigen::MatrixXf& inputMap, const Eigen::Vector2i& source) const;
+        Result spfa(const Eigen::MatrixXf& inputMap,
+                    const Eigen::Vector2i& source,
+                    bool checkStartForCollision = true) const;
 
 
     private: