Add tests for path generation

ethz-asl · Dec 2, 2024 · 148aff6 · 148aff6
1 parent 7f63132
commit 148aff6
Show file tree

Hide file tree

Showing 5 changed files with 46 additions and 1 deletion.
diff --git a/terrain_abort_planner/CMakeLists.txt b/terrain_abort_planner/CMakeLists.txt
@@ -50,3 +50,17 @@ add_executable(run_dynamic_rallypoints
 )
 add_dependencies(run_dynamic_rallypoints ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(run_dynamic_rallypoints ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES} nlohmann_json::nlohmann_json)
+
+if(CATKIN_ENABLE_TESTING)
+    # Add gtest based cpp test target and link libraries
+    catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp
+                                          test/test_mission_io.cpp)
+                                          # test/test_terrain_map.cpp)
+
+    if(TARGET ${PROJECT_NAME}-test)
+        target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}
+                                                 ${catkin_LIBRARIES}
+                                                 ${YAML_CPP_LIBRARIES} ${OpenCV_LIBRARIES})
+    endif()
+
+endif()
diff --git a/terrain_abort_planner/include/terrain_abort_planner/mission_io.h b/terrain_abort_planner/include/terrain_abort_planner/mission_io.h
@@ -95,3 +95,8 @@ PathSegment generateTrajectory(const double curvature, const double length, Eige
  * @return Path 
  */
 Path generatePathFromWaypoints (std::vector<Eigen::Vector3d> waypoint_list, const double radius);
+
+double getTurnAngle(double current_course, double next_course) {
+    double turn_angle = std::abs(M_PI - std::abs(next_course - current_course));
+    return turn_angle;
+}
diff --git a/terrain_abort_planner/src/mission_io.cpp b/terrain_abort_planner/src/mission_io.cpp
@@ -17,7 +17,7 @@ Path generatePathFromWaypoints (std::vector<Eigen::Vector3d> waypoint_list, cons
     Eigen::Vector3d next_tangent = next_edge / next_edge_length;
     double next_course = std::atan2(next_tangent[1], next_tangent[0]);
 
-    double turn_angle = M_PI - std::abs(next_course - current_course);
+    double turn_angle = getTurnAngle(current_course, next_course);
     double distance_to_tangent = radius / std::abs(std::tan(0.5 * turn_angle));
     std::cout << "turn_angle: " << turn_angle / M_PI << " pi" << std::endl;
     std::cout << "  - current_edge_length: " << current_edge_length << std::endl;

diff --git a/terrain_abort_planner/test/main.cpp b/terrain_abort_planner/test/main.cpp
@@ -0,0 +1,6 @@
+#include <gtest/gtest.h>
+
+int main(int argc, char **argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/terrain_abort_planner/test/test_mission_io.cpp b/terrain_abort_planner/test/test_mission_io.cpp
@@ -0,0 +1,20 @@
+#include "terrain_abort_planner/mission_io.h"
+
+#include <gtest/gtest.h>
+#include <iostream>
+
+TEST(MissionIO, PathGeneration) {
+  EXPECT_NEAR(getTurnAngle(0.25 * M_PI, 0.25*M_PI), M_PI, 0.001);
+  EXPECT_NEAR(getTurnAngle(0.0, 0.5*M_PI), 0.5*M_PI, 0.001);
+  EXPECT_NEAR(getTurnAngle(0.0, -0.5*M_PI), 0.5*M_PI, 0.001);
+  EXPECT_NEAR(getTurnAngle(0.75 * M_PI, -0.75 * M_PI), 0.5*M_PI, 0.001);
+
+  // EXPECT_NEAR(getTurnAngle(0.25 * M_PI, 0.25*M_PI), M_PI, 0.001);
+  // EXPECT_NEAR(getTurnAngle(0.25 * M_PI, 0.25*M_PI), M_PI, 0.001);
+  // EXPECT_NEAR(getTurnAngle(0.25 * M_PI, 0.25*M_PI), M_PI, 0.001);
+
+
+  std::vector<Eigen::Vector3d> waypoint_list;
+  double radius = 1.0;
+
+}