feat(freespace_planning_algorithms): add tests for updated astar algorithm

planning/freespace_planning_algorithms/CMakeLists.txt

@@ -34,8 +34,12 @@ if(BUILD_TESTING)
   target_link_libraries(freespace_planning_algorithms-test
     freespace_planning_algorithms
   )
-
-  find_package(ament_cmake_ros REQUIRED)
+  ament_add_ros_isolated_gtest(freespace_planning_algorithms_no_obs-test
+    test/src/test_freespace_planning_algorithms_no_obstacles.cpp
+  )
+  target_link_libraries(freespace_planning_algorithms_no_obs-test
+    freespace_planning_algorithms
+  )
   ament_add_gtest(rrtstar_core_informed-test
     test/src/test_rrtstar_core_informed.cpp
   )

planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc. All rights reserved.
+// Copyright 2021 TIER IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -134,7 +134,6 @@ nav_msgs::msg::OccupancyGrid construct_cost_map(
       }
     }
   }
-  return costmap_msg;
 
   return costmap_msg;
 }
@@ -210,9 +209,12 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_astar(bool use_multi)
   // configure astar param
   const bool only_behind_solutions = false;
   const bool use_back = true;
+  const bool use_curve_weight = true;
+  const bool use_complete_astar = true;
   const double distance_heuristic_weight = 1.0;
-  const auto astar_param =
-    fpa::AstarParam{only_behind_solutions, use_back, distance_heuristic_weight};
+  const auto astar_param = fpa::AstarParam{
+    only_behind_solutions, use_back, use_curve_weight, use_complete_astar,
+    distance_heuristic_weight};
 
   auto algo = std::make_unique<fpa::AstarSearch>(planner_common_param, vehicle_shape, astar_param);
   return algo;