smoother obstacle cost function

MRPT · Sep 23, 2024 · ee38778 · ee38778
1 parent 538ec8c
commit ee38778
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/mrpt_path_planning/src/algos/CostEvaluatorCostMap.cpp b/mrpt_path_planning/src/algos/CostEvaluatorCostMap.cpp
@@ -97,7 +97,7 @@ CostEvaluatorCostMap::Ptr CostEvaluatorCostMap::FromStaticPointObstacles(
             if (d < D)
             {
                 const auto cost =
-                    p.maxCost * std::pow(-0.99999 + 1. / (d / D), 0.1);
+                    p.maxCost * std::pow(-0.99999 + 1. / (d / D), 0.4);
                 ASSERT_GE_(cost, .0);
 
                 double* cell = g.cellByIndex(cx, cy);
@@ -123,7 +123,8 @@ double CostEvaluatorCostMap::operator()(const MoveEdgeSE2_TPS& edge) const
     double cost = .0;
     size_t n    = 0;
 
-    auto lambdaAddPose = [this, &cost, &n](const mrpt::math::TPose2D& p) {
+    auto lambdaAddPose = [this, &cost, &n](const mrpt::math::TPose2D& p)
+    {
         const auto c = eval_single_pose(p);
         ASSERT_GE_(c, .0);