diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp
index 28bc5aa141498..081fbf6920bc0 100644
--- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp
+++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp
@@ -40,16 +40,12 @@ const double width_lexus = 2.75;
 const fpa::VehicleShape vehicle_shape = fpa::VehicleShape{length_lexus, width_lexus, 1.5};
 const double pi = 3.1415926;
 const std::array<double, 3> start_pose{5.5, 4., pi * 0.5};
-const std::array<double, 3> goal_pose1{8.0, 26.3, pi * 1.5};  // easiest
-const std::array<std::array<double, 3>, 1> goal_poses{goal_pose1};
-
-// the tests for following goals randomly fail. needs to be fixed.
-// https://github.com/autowarefoundation/autoware.universe/issues/2439
-// const std::array<double, 3> goal_pose2{15.0, 11.6, pi * 0.5};  // second easiest
-// const std::array<double, 3> goal_pose3{18.4, 26.3, pi * 1.5};  // third easiest
-// const std::array<double, 3> goal_pose4{25.0, 26.3, pi * 1.5};  // most difficult
-// const std::array<std::array<double, 3>, 4> goal_poses{
-//   goal_pose1, goal_pose2, goal_pose3, goal_pose4};
+const std::array<double, 3> goal_pose1{8.0, 26.3, pi * 1.5};   // easiest
+const std::array<double, 3> goal_pose2{15.0, 11.6, pi * 0.5};  // second easiest
+const std::array<double, 3> goal_pose3{18.4, 26.3, pi * 1.5};  // third easiest
+const std::array<double, 3> goal_pose4{25.0, 26.3, pi * 1.5};  // most difficult
+const std::array<std::array<double, 3>, 4> goal_poses{
+  goal_pose1, goal_pose2, goal_pose3, goal_pose4};
 
 geometry_msgs::msg::Pose create_pose_msg(std::array<double, 3> pose3d)
 {
@@ -250,7 +246,7 @@ std::unordered_map<AlgorithmType, std::string> rosbag_dir_prefix_table(
    {RRTSTAR_UPDATE, "fpalgos-rrtstar_update"},
    {RRTSTAR_INFORMED_UPDATE, "fpalgos-rrtstar_informed_update"}});
 
-bool test_algorithm(enum AlgorithmType algo_type)
+bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false)
 {
   std::unique_ptr<fpa::AbstractPlanningAlgorithm> algo;
   if (algo_type == AlgorithmType::ASTAR_SINGLE) {
@@ -273,8 +269,6 @@ bool test_algorithm(enum AlgorithmType algo_type)
 
   rclcpp::Clock clock{RCL_SYSTEM_TIME};
   for (size_t i = 0; i < goal_poses.size(); ++i) {
-    const std::string dir_name =
-      "/tmp/" + rosbag_dir_prefix_table[algo_type] + "-case" + std::to_string(i);
     const auto goal_pose = goal_poses.at(i);
 
     bool success_local = true;
@@ -325,41 +319,46 @@ bool test_algorithm(enum AlgorithmType algo_type)
       success_all = false;
     }
 
-    rcpputils::fs::remove_all(dir_name);
-
-    rosbag2_storage::StorageOptions storage_options;
-    storage_options.uri = dir_name;
-    storage_options.storage_id = "sqlite3";
-
-    rosbag2_cpp::ConverterOptions converter_options;
-    converter_options.input_serialization_format = "cdr";
-    converter_options.output_serialization_format = "cdr";
-
-    rosbag2_cpp::Writer writer(std::make_unique<rosbag2_cpp::writers::SequentialWriter>());
-    writer.open(storage_options, converter_options);
-
-    add_message_to_rosbag(
-      writer, create_float_msg(vehicle_shape.length), "vehicle_length", "std_msgs/msg/Float64");
-    add_message_to_rosbag(
-      writer, create_float_msg(vehicle_shape.width), "vehicle_width", "std_msgs/msg/Float64");
-    add_message_to_rosbag(
-      writer, create_float_msg(vehicle_shape.base2back), "vehicle_base2back",
-      "std_msgs/msg/Float64");
-
-    add_message_to_rosbag(writer, costmap_msg, "costmap", "nav_msgs/msg/OccupancyGrid");
-    add_message_to_rosbag(writer, create_pose_msg(start_pose), "start", "geometry_msgs/msg/Pose");
-    add_message_to_rosbag(writer, create_pose_msg(goal_pose), "goal", "geometry_msgs/msg/Pose");
-    add_message_to_rosbag(writer, trajectory, "trajectory", "geometry_msgs/msg/PoseArray");
-    add_message_to_rosbag(writer, create_float_msg(msec), "elapsed_time", "std_msgs/msg/Float64");
+    if (dump_rosbag) {
+      // dump rosbag for visualization using python script
+      const std::string dir_name =
+        "/tmp/" + rosbag_dir_prefix_table[algo_type] + "-case" + std::to_string(i);
+
+      rcpputils::fs::remove_all(dir_name);
+
+      rosbag2_storage::StorageOptions storage_options;
+      storage_options.uri = dir_name;
+      storage_options.storage_id = "sqlite3";
+
+      rosbag2_cpp::ConverterOptions converter_options;
+      converter_options.input_serialization_format = "cdr";
+      converter_options.output_serialization_format = "cdr";
+
+      rosbag2_cpp::Writer writer(std::make_unique<rosbag2_cpp::writers::SequentialWriter>());
+      writer.open(storage_options, converter_options);
+
+      add_message_to_rosbag(
+        writer, create_float_msg(vehicle_shape.length), "vehicle_length", "std_msgs/msg/Float64");
+      add_message_to_rosbag(
+        writer, create_float_msg(vehicle_shape.width), "vehicle_width", "std_msgs/msg/Float64");
+      add_message_to_rosbag(
+        writer, create_float_msg(vehicle_shape.base2back), "vehicle_base2back",
+        "std_msgs/msg/Float64");
+
+      add_message_to_rosbag(writer, costmap_msg, "costmap", "nav_msgs/msg/OccupancyGrid");
+      add_message_to_rosbag(writer, create_pose_msg(start_pose), "start", "geometry_msgs/msg/Pose");
+      add_message_to_rosbag(writer, create_pose_msg(goal_pose), "goal", "geometry_msgs/msg/Pose");
+      add_message_to_rosbag(writer, trajectory, "trajectory", "geometry_msgs/msg/PoseArray");
+      add_message_to_rosbag(writer, create_float_msg(msec), "elapsed_time", "std_msgs/msg/Float64");
+    }
   }
   return success_all;
 }
 
-// the following test fails https://github.com/autowarefoundation/autoware.universe/issues/2439
-// TEST(AstarSearchTestSuite, SingleCurvature)
-// {
-//   EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_SINGLE));
-// }
+TEST(AstarSearchTestSuite, SingleCurvature)
+{
+  EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_SINGLE));
+}
 
 TEST(AstarSearchTestSuite, MultiCurvature)
 {