planner_cspace: add new parameters for cost function (#720)

at-wat · Sep 13, 2023 · fdfea8e · fdfea8e
1 parent 600e7bb
commit fdfea8e
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 6 deletions.
diff --git a/planner_cspace/cfg/Planner3D.cfg b/planner_cspace/cfg/Planner3D.cfg
@@ -19,8 +19,10 @@ gen.add("weight_backward", double_t, 0, "", 0.9, 0.0, 1000.0)
 gen.add("weight_ang_vel", double_t, 0, "", 1.0, 0.0, 1000.0)
 gen.add("weight_costmap", double_t, 0, "", 50.0, 0.0, 1000.0)
 gen.add("weight_costmap_turn", double_t, 0, "", 0.0, 0.0, 1000.0)
+gen.add("weight_costmap_turn_heuristics", double_t, 0, "The weight of the heuristic cost of in-place turning at grid cells with costs", 100.0, 0.0, 1000.0)
 gen.add("weight_remembered", double_t, 0, "", 1000.0, 0.0, 1000.0)
 gen.add("cost_in_place_turn", double_t, 0, "", 30.0, 0.0, 1000.0)
+gen.add("turn_penalty_cost_threshold", int_t, 0, "Penalty costs of in-place turning are not added when the cost of the grid cell is lower than this value", 0, 0, 100)
 gen.add("hysteresis_max_dist", double_t, 0, "", 0.1, 0.0, 10.0)
 gen.add("hysteresis_expand", double_t, 0, "", 0.1, 0.0, 10.0)
 gen.add("weight_hysteresis", double_t, 0, "", 5.0, 0.0, 1000.0)

diff --git a/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h b/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h
@@ -56,6 +56,7 @@ class CostCoeff
   float weight_ang_vel_;
   float weight_costmap_;
   float weight_costmap_turn_;
+  float weight_costmap_turn_heuristics_;
   float weight_remembered_;
   float weight_hysteresis_;
   float in_place_turn_;
@@ -65,6 +66,7 @@ class CostCoeff
   float max_vel_;
   float max_ang_vel_;
   float angle_resolution_aspect_;
+  int turn_penalty_cost_threshold_;
 };
 
 class GridAstarModel3D : public GridAstarModelBase<3, 2>

diff --git a/planner_cspace/src/grid_astar_model_3dof.cpp b/planner_cspace/src/grid_astar_model_3dof.cpp
@@ -176,14 +176,18 @@ float GridAstarModel3D::cost(
       const auto c = cm_[pos];
       if (c > 99)
         return -1;
-      sum += c;
+      if (c >= cc_.turn_penalty_cost_threshold_)
+      {
+        sum += c;
+      }
     }
-
-    cost +=
-        sum * map_info_.angular_resolution * euclid_cost_coef_[2] / euclid_cost_coef_[0] +
-        sum * map_info_.angular_resolution * cc_.weight_costmap_turn_ / 100.0;
+    const float turn_cost_ratio = cc_.weight_costmap_turn_ / 100.0;
+    const float turn_heuristic_cost_ratio =
+        euclid_cost_coef_[2] / euclid_cost_coef_[0] * cc_.weight_costmap_turn_heuristics_ / 100.0;
+    const float turn_cost_multiplier = map_info_.angular_resolution * (turn_cost_ratio + turn_heuristic_cost_ratio);
     // simplified from sum * map_info_.angular_resolution * abs(d[2]) * cc_.weight_costmap_turn_ / (100.0 * abs(d[2]))
-    return cc_.in_place_turn_ + cost;
+    //               = sum * map_info_.angular_resolution * turn_cost_ratio
+    return cost + cc_.in_place_turn_ + sum * turn_cost_multiplier;
   }
 
   const Vec d2(d[0] + range_, d[1] + range_, next[2]);

diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
@@ -1297,6 +1297,8 @@ class Planner3dNode
     cc_.hysteresis_max_dist_ = config.hysteresis_max_dist;
     cc_.hysteresis_expand_ = config.hysteresis_expand;
     cc_.weight_hysteresis_ = config.weight_hysteresis;
+    cc_.weight_costmap_turn_heuristics_ = config.weight_costmap_turn_heuristics;
+    cc_.turn_penalty_cost_threshold_ = config.turn_penalty_cost_threshold;
 
     goal_tolerance_lin_f_ = config.goal_tolerance_lin;
     goal_tolerance_ang_f_ = config.goal_tolerance_ang;