Add more test

planner_cspace/test/src/test_planner_3d_cost.cpp

@@ -66,6 +66,7 @@ TEST(GridAstarModel3D, Cost)
   cc.max_vel_ = 1.0;
   cc.max_ang_vel_ = 1.0;
   cc.angle_resolution_aspect_ = 1.0;
+  cc.turn_penalty_cost_threshold_ = 0;
 
   GridAstarModel3D model(
       map_info,
@@ -92,7 +93,7 @@ TEST(GridAstarModel3D, Cost)
   EXPECT_LT(c_straight, c_drift);
   EXPECT_LT(c_straight, c_drift_curve);
 
-  // These tests are added to confirm a bug is fixed by https://github.com/at-wat/neonavigation/pull/725
+  // These tests are added to confirm that some bugs are fixed by https://github.com/at-wat/neonavigation/pull/725
   const GridAstarModel3D::Vec start2(5, 5, 0);
   const GridAstarModel3D::Vec occupied(10, 5, 0);
   const GridAstarModel3D::Vec goal2(10, 5, 3);
@@ -101,6 +102,15 @@ TEST(GridAstarModel3D, Cost)
   EXPECT_LT(model.cost(start2, occupied, {GridAstarModel3D::VecWithCost(start2)}, occupied), 0);
   // The cost from the occupied cell should be negative.
   EXPECT_LT(model.cost(occupied, goal2, {GridAstarModel3D::VecWithCost(occupied)}, goal2), 0);
+
+  const GridAstarModel3D::Vec start3(10, 20, 0);
+  cm[start3] = 99;
+  const GridAstarModel3D::Vec waypoint3(10, 20, 3);
+  const GridAstarModel3D::Vec goal3(10, 20, 6);
+  // The cost between start3 and waypoint3 is larger than the cost between waypoint3 and goal3 because start3
+  // has a penalty.
+  EXPECT_GT(model.cost(start3, waypoint3, {GridAstarModel3D::VecWithCost(start3)}, waypoint3),
+            model.cost(waypoint3, waypoint3, {GridAstarModel3D::VecWithCost(goal3)}, goal3));
 }
 }  // namespace planner_3d
 }  // namespace planner_cspace