From d6dc8941db6c3ab210944e54223dbc29828ebe06 Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Fri, 18 Aug 2023 11:07:39 -0400 Subject: [PATCH 1/6] Remove boost header --- include/trochoids/DubinsStateSpace.h | 2 +- src/DubinsStateSpace.cpp | 2 +- test/unit_test_trochoids.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/trochoids/DubinsStateSpace.h b/include/trochoids/DubinsStateSpace.h index eb73df4..f3ee9ca 100644 --- a/include/trochoids/DubinsStateSpace.h +++ b/include/trochoids/DubinsStateSpace.h @@ -38,7 +38,7 @@ #ifndef TROCHOIDS_DUBINSSTATESPACE_H #define TROCHOIDS_DUBINSSTATESPACE_H -#include +#include #include #include #include diff --git a/src/DubinsStateSpace.cpp b/src/DubinsStateSpace.cpp index 80fe0f2..2f89022 100644 --- a/src/DubinsStateSpace.cpp +++ b/src/DubinsStateSpace.cpp @@ -41,7 +41,7 @@ namespace Dubins { - const double twopi = 2. * boost::math::constants::pi(); + const double twopi = 2. * M_PI; const double DUBINS_EPS = 1e-6; const double DUBINS_ZERO = -1e-7; diff --git a/test/unit_test_trochoids.cpp b/test/unit_test_trochoids.cpp index 37af30d..79ffaab 100644 --- a/test/unit_test_trochoids.cpp +++ b/test/unit_test_trochoids.cpp @@ -1370,7 +1370,7 @@ TEST(TestTrochoids, trochoid_analytical_failure_case1) trochoid_path.clear(); bool valid_numerical = trochoids::get_trochoid_path_numerical(start_state, goal_state, trochoid_path, wind, desired_speed, max_kappa); - double dist_numerical; + double dist_numerical = 0; if (valid_numerical) dist_numerical = trochoids::get_length(trochoid_path); EXPECT_TRUE(valid_numerical); From 68f2f709f08434addf4c02b237930608f647f36b Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Mon, 28 Aug 2023 15:11:34 -0400 Subject: [PATCH 2/6] Update long distance condition for dubins solve --- src/DubinsStateSpace.cpp | 11 +++- src/trochoids.cpp | 2 +- test/unit_test_trochoid_classification.cpp | 59 ++++++++++++++++++++++ 3 files changed, 69 insertions(+), 3 deletions(-) diff --git a/src/DubinsStateSpace.cpp b/src/DubinsStateSpace.cpp index 2f89022..5543cc4 100644 --- a/src/DubinsStateSpace.cpp +++ b/src/DubinsStateSpace.cpp @@ -236,6 +236,13 @@ int Dubins::find_quadrant(double angle) return 4; // fourth quadrant } +/** + * @brief Uses the Dubins set classification for solving a Dubins path + * + * @param state1 DubinsState of the starting pose + * @param state2 DubinsState of the ending pose + * @return DubinsPath + */ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(const DubinsState state1, const DubinsState state2) const { @@ -243,8 +250,8 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con double x2 = state2.x, y2 = state2.y, th2 = state2.theta; double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / rho_, th = atan2(dy, dx); double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th); - // if (d > (sqrt(4 - pow((abs(cos(alpha)) + abs(cos(beta))),2)) + abs(sin(alpha)) + abs(sin(beta)))) - if (d > 4) + if (d > (std::abs(std::sin(alpha)) + std::abs(std::sin(beta)) + + std::sqrt(4 - std::pow(std::cos(alpha) + std::cos(beta), 2)))) { int init_quadrant = find_quadrant(alpha); int final_quadrant = find_quadrant(beta); diff --git a/src/trochoids.cpp b/src/trochoids.cpp index 832870e..9863fc5 100755 --- a/src/trochoids.cpp +++ b/src/trochoids.cpp @@ -371,7 +371,7 @@ std::vector> trochoids::Trochoid::trochoid_classificat } } // Compute the quadrant between the decision points - if (std::isnan(d_between_angle)) // No decision points + if (std::isnan(d_between_angle)) // No decision points or at last decision point { bool within_four_r = check_within_four_r(decision_points[decision_points.size()-1], x0, y0, xf, yf); if (within_four_r) diff --git a/test/unit_test_trochoid_classification.cpp b/test/unit_test_trochoid_classification.cpp index 0c74fa0..ffa0248 100644 --- a/test/unit_test_trochoid_classification.cpp +++ b/test/unit_test_trochoid_classification.cpp @@ -313,6 +313,9 @@ TEST_F(DubinsTestFixture, dubins_matrix_test_random) std::cout << "Dubins Path Length: " << dubins_path_length << std::endl; std::cout << "Max_Kappa: " << max_kappa << std::endl; + std::cout << "Start State: " << start_state.x << ", " << start_state.y << ", " << start_state.theta << std::endl; + std::cout << "Goal State: " << goal_state.x << ", " << goal_state.y << ", " << goal_state.theta << std::endl; + std::cout << "Dubins Matrix Path Type: " << dubins_path_matrix.type_[0] << ", " << dubins_path_matrix.type_[1] << ", " << dubins_path_matrix.type_[2] << std::endl; std::cout << "Dubins Path Type: " << dubins_path.type_[0] << ", " << dubins_path.type_[1] << ", " << dubins_path.type_[2] << std::endl; } @@ -428,6 +431,62 @@ TEST_F(DubinsTestFixture, failure_case_dubins_matrix4) // std::cout << "dubins_path_type: " << dubins_path.type_[0] << ", " << dubins_path.type_[1] << ", " << dubins_path.type_[2] << std::endl; } +TEST_F(DubinsTestFixture, failure_case_dubins_four_r1_1) +{ + /* + Dubins Matrix Path Length: 8.26153 + Dubins Path Length: 8.07563 + Max_Kappa: 0.00572078 + Start State: -51.074, -287.227, 0.888931 + Goal State: -607.936, -471.39, 1.94105 + Dubins Matrix Path Type: 2, 1, 2 + Dubins Path Type: 2, 0, 2 + */ + max_kappa = 0.00572078; + // Run dubins on this (should choose dubinsLSL and segfault) + + Dubins::DubinsStateSpace::DubinsState start_state = {-51.074, -287.227, 0.888931}; + Dubins::DubinsStateSpace::DubinsState goal_state = {-607.936, -471.39, 1.94105}; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + dubins_path_matrix = dubins_path_object.dubins_matrix(start_state, goal_state); + dubins_path = dubins_path_object.dubins(start_state, goal_state); + + double dubins_matrix_path_length = dubins_path_matrix.length(); + double dubins_path_length = dubins_path.length(); + + EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5); +} + +TEST_F(DubinsTestFixture, failure_case_dubins_four_r1_2) +{ + /* + Dubins Matrix Path Length: 1.79769e+308 + Dubins Path Length: 6.60756 + Max_Kappa: 0.00982667 + Start State: -347.852, 202.189, 3.46437 + Goal State: -505.575, 148.801, 0.333584 + Dubins Matrix Path Type: 0, 1, 0 + Dubins Path Type: 2, 0, 2 + */ + max_kappa = 0.00982667; + // Run dubins on this (should choose dubinsLSL and segfault) + + Dubins::DubinsStateSpace::DubinsState start_state = {-347.852, 202.189, 3.46437}; + Dubins::DubinsStateSpace::DubinsState goal_state = {-505.575, 148.801, 0.333584}; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + dubins_path_matrix = dubins_path_object.dubins_matrix(start_state, goal_state); + dubins_path = dubins_path_object.dubins(start_state, goal_state); + + double dubins_matrix_path_length = dubins_path_matrix.length(); + double dubins_path_length = dubins_path.length(); + + EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5); +} + TEST_F(DubinsTestFixture, fail_case_8_tests) { max_kappa = 0.066222699999999995; From ac52d6f01dbd69cd24f6f171edf2ef6121779f7b Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Mon, 28 Aug 2023 16:04:10 -0400 Subject: [PATCH 3/6] Add benchmarks for dubins solving --- test/all_benchmarks.cpp | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/test/all_benchmarks.cpp b/test/all_benchmarks.cpp index d02b6ee..ff1371b 100644 --- a/test/all_benchmarks.cpp +++ b/test/all_benchmarks.cpp @@ -445,5 +445,41 @@ static void BM_GetTrochoid_with_classification(benchmark::State& state) { } BENCHMARK(BM_GetTrochoid_with_classification); +static void BM_Dubins_Random_Exhaustive(benchmark::State& state) { + std::random_device rd; + std::mt19937 gen = std::mt19937(rd()); + std::uniform_real_distribution<> disRange(-1000, 1000); + std::uniform_real_distribution<> disPhi(0.0, 2.0 * M_PI); + + double max_kappa = 0.01; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + for (auto _ : state){ + benchmark::DoNotOptimize(dubins_path_object.dubins({disRange(gen), disRange(gen), disPhi(gen)}, {disRange(gen), disRange(gen), disPhi(gen)})); + benchmark::ClobberMemory(); + } + +} +BENCHMARK(BM_Dubins_Random_Exhaustive); + +static void BM_Dubins_Random_Classification(benchmark::State& state) { + std::random_device rd; + std::mt19937 gen = std::mt19937(rd()); + std::uniform_real_distribution<> disRange(-1000, 1000); + std::uniform_real_distribution<> disPhi(0.0, 2.0 * M_PI); + + double max_kappa = 0.01; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + for (auto _ : state){ + benchmark::DoNotOptimize(dubins_path_object.dubins_matrix({disRange(gen), disRange(gen), disPhi(gen)}, {disRange(gen), disRange(gen), disPhi(gen)})); + benchmark::ClobberMemory(); + } + +} +BENCHMARK(BM_Dubins_Random_Classification); + BENCHMARK_MAIN(); \ No newline at end of file From 319796334a099552c120d9e8990694b23598ed36 Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Tue, 19 Sep 2023 16:10:21 -0400 Subject: [PATCH 4/6] README updates --- README.md | 28 ++++------------------------ 1 file changed, 4 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index 151dc98..3c7b1bc 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ # Time-Optimal Path Planning in a Constant Wind for Uncrewed Aerial Vehicles using Dubins Set Classification This repository contains code for the paper -**"Time-Optimal Path Planning in a Constant Wind for Uncrewed Aerial Vehicles using Dubins Set Classification"** by *Sagar Sachdev\*, Brady Moon\*, Junbin Yuan, and Sebastian Scherer (\* equal contribution)*. +**"Time-Optimal Path Planning in a Constant Wind for Uncrewed Aerial Vehicles using Dubins Set Classification"** by *Brady Moon\*, Sagar Sachdev\*, Junbin Yuan, and Sebastian Scherer (\* equal contribution)*. This codebase includes both a solver for trochoidal paths when there is wind as well as also solving Dubins paths when there is no wind. The Dubins path solutions use the work "Classification of the Dubins set" as well as the correction proposed in the work "Circling Back: Dubins set Classification Revisited." @@ -11,27 +11,7 @@ This codebase includes both a solver for trochoidal paths when there is wind as

## Brief Overview - Time-optimal path planning in high winds for a -turning rate constrained UAV is a challenging problem to solve -and is important for deployment and field operations. Previous -works have used trochoidal path segments, which consist of -straight and maximum-rate turn segments, as optimal extremal -paths in uniform wind conditions. Current methods iterate -over all candidate trochoidal trajectory types and choose the -time-optimal one; however, this can be computationally slow. -As such, a method to narrow down the candidate trochoidal -trajectory types before computing the trajectories would reduce -the computation time. We thus introduce a geometric -approach to reduce the candidate trochoidal trajectory types by -framing the problem in the air-relative frame and bounding the -solution within a subset of candidate trajectories. This method -reduces overall computation by around 37% compared to pre- -existing methods in Bang-Straight-Bang trajectories, freeing -up computation for other onboard processes and can lead to -significant total computational reductions when solving many -trochoidal paths. When used within the framework of a global -path planner, faster state expansions help find solutions faster or -compute higher-quality paths. +Time-optimal path planning in high winds for a turning-rate constrained UAV is a challenging problem to solve and is important for deployment and field operations. Previous works have used trochoidal path segments comprising straight and maximum-rate turn segments, as optimal extremal paths in uniform wind conditions. Current methods iterate over all candidate trochoidal trajectory types and select the one that is time-optimal; however, this exhaustive search can be computationally slow. In this paper, we introduce a method to decrease the computation time. This is achieved by reducing the number of candidate trochoidal trajectory types by framing the problem in the air-relative frame and bounding the solution within a subset of candidate trajectories. Our method reduces overall computation by 37.4% compared to pre-existing methods in Bang-Straight-Bang trajectories, freeing up computation for other onboard processes and can lead to significant total computational reductions when solving many trochoidal paths. When used within the framework of a global path planner, faster state expansions help find solutions faster or compute higher-quality paths. We also release our open-source codebase as a C++ package. ## Prerequisites @@ -107,9 +87,9 @@ bool valid = trochoids::get_trochoid_path(start_state, goal_state, trochoid_path ## Citation If you find this work useful, please cite our paper: ``` -@article{sachdev2023timeoptimal, +@article{moon2023timeoptimal, title={Time-Optimal Path Planning in a Constant Wind for Uncrewed Aerial Vehicles using Dubins Set Classification}, - author={Sagar Sachdev and Brady Moon and Junbin Yuan and Sebastian Scherer}, + author={Brady Moon and Sagar Sachdev and Junbin Yuan and Sebastian Scherer}, year={2023}, eprint={2306.11845}, archivePrefix={arXiv}, From e4c71c418af1ff2e231760046eca7bcbe12166dd Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Tue, 19 Sep 2023 16:14:37 -0400 Subject: [PATCH 5/6] Dubins Classification Speed ups - Changes to optimize a_22/a_33 - Minor changes in other Dubins classifications and removed some default edge cases where it would resort to all path types. --- src/DubinsStateSpace.cpp | 67 ++++++----------- test/all_benchmarks.cpp | 59 +++++++++++++++ test/unit_test_trochoid_classification.cpp | 84 ++++++++++++++++++++++ 3 files changed, 166 insertions(+), 44 deletions(-) diff --git a/src/DubinsStateSpace.cpp b/src/DubinsStateSpace.cpp index 5543cc4..2257753 100644 --- a/src/DubinsStateSpace.cpp +++ b/src/DubinsStateSpace.cpp @@ -278,10 +278,10 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta); DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta); double S13 = dubinsrsr.length_[0] - M_PI; - double S12 = dubinsrsr.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[2] - M_PI); if (S13 <= 0) { + double S12 = dubinsrsr.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[2] - M_PI); if (S12 > 0) { return dubinsrsl; @@ -327,10 +327,10 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con DubinsPath dubinslsl = dubinsLSL(d, alpha, beta); DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta); double S31 = dubinslsl.length_[2] - M_PI; - double S21 = dubinslsl.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[0] - M_PI); if (S31 <= 0) { + double S21 = dubinslsl.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[0] - M_PI); if (S21 > 0) { return dubinsrsl; @@ -362,13 +362,9 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con { return dubinsrsr; } - else if (S24 > 0) - { - return dubinsRSL(d, alpha, beta); - } else { - return Dubins::dubins(d, alpha, beta); + return dubinsRSL(d, alpha, beta); } } else if (init_quadrant == 3 && final_quadrant == 1) @@ -380,13 +376,9 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con { return dubinslsl; } - else if (S31 > 0) - { - return dubinsLSR(d, alpha, beta); - } else { - return Dubins::dubins(d, alpha, beta); + return dubinsLSR(d, alpha, beta); } } else if (init_quadrant == 3 && final_quadrant == 4) @@ -394,10 +386,10 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con DubinsPath dubinslsr = dubinsLSR(d, alpha, beta); DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta); double S24 = dubinsrsr.length_[2] - M_PI; - double S34 = dubinsrsr.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[0] - M_PI); if (S24 <= 0) { + double S34 = dubinsrsr.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[0] - M_PI); if (S34 > 0) { return dubinslsr; @@ -429,13 +421,9 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con { return dubinslsl; } - else if (S42 > 0) - { - return dubinsRSL(d, alpha, beta); - } else { - return Dubins::dubins(d, alpha, beta); + return dubinsRSL(d, alpha, beta); } } else if (init_quadrant == 4 && final_quadrant == 3) @@ -443,10 +431,10 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con DubinsPath dubinslsr = dubinsLSR(d, alpha, beta); DubinsPath dubinslsl = dubinsLSL(d, alpha, beta); double S42 = dubinslsl.length_[0] - M_PI; - double S43 = dubinslsl.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[2] - M_PI); if (S42 <= 0) { + double S43 = dubinslsl.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[2] - M_PI); if (S43 > 0) { return dubinslsr; @@ -492,18 +480,14 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con } else if (init_quadrant == 2 && final_quadrant == 2) { - DubinsPath dubinslsl = dubinsLSL(d, alpha, beta); DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta); - DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta); - - double plsl = dubinslsl.length_[1]; double prsl = dubinsrsl.length_[1]; - double trsl = dubinsrsl.length_[0]; - double prsr = dubinsrsr.length_[1]; - double qrsl = dubinsrsl.length_[2]; - + if (alpha > beta) { + DubinsPath dubinslsl = dubinsLSL(d, alpha, beta); + double trsl = dubinsrsl.length_[0]; + double plsl = dubinslsl.length_[1]; double S122 = plsl - prsl - 2*(trsl - M_PI); if (S122 < 0) { @@ -518,8 +502,11 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con return Dubins::dubins(d, alpha, beta); } } - else if (alpha < beta) + else { + DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta); + double prsr = dubinsrsr.length_[1]; + double qrsl = dubinsrsl.length_[2]; double S222 = prsr - prsl - 2*(qrsl - M_PI); if (S222 < 0) { @@ -534,24 +521,17 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con return Dubins::dubins(d, alpha, beta); } } - else - { - return Dubins::dubins(d, alpha, beta); - } } else if (init_quadrant == 3 && final_quadrant == 3) { - DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta); DubinsPath dubinslsr = dubinsLSR(d, alpha, beta); - DubinsPath dubinslsl = dubinsLSL(d, alpha, beta); - - double prsr = dubinsrsr.length_[1]; double plsr = dubinslsr.length_[1]; - double tlsr = dubinslsr.length_[0]; - double plsl = dubinslsl.length_[1]; - double qlsr = dubinslsr.length_[2]; + if (alpha < beta) { + DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta); + double prsr = dubinsrsr.length_[1]; + double tlsr = dubinslsr.length_[0]; double S133 = prsr - plsr - 2*(tlsr - M_PI); if (S133 < 0) { @@ -566,8 +546,11 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con return Dubins::dubins(d, alpha, beta); } } - else if (alpha > beta) + else { + DubinsPath dubinslsl = dubinsLSL(d, alpha, beta); + double plsl = dubinslsl.length_[1]; + double qlsr = dubinslsr.length_[2]; double S233 = plsl - plsr - 2*(qlsr - M_PI); if (S233 < 0) { @@ -582,10 +565,6 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con return Dubins::dubins(d, alpha, beta); } } - else - { - return Dubins::dubins(d, alpha, beta); - } } else if (init_quadrant == 4 && final_quadrant == 1) { diff --git a/test/all_benchmarks.cpp b/test/all_benchmarks.cpp index ff1371b..af6a7fe 100644 --- a/test/all_benchmarks.cpp +++ b/test/all_benchmarks.cpp @@ -481,5 +481,64 @@ static void BM_Dubins_Random_Classification(benchmark::State& state) { } BENCHMARK(BM_Dubins_Random_Classification); +static void BM_Dubins_a12(benchmark::State& state) { + double max_kappa = 0.01; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + for (auto _ : state){ + benchmark::DoNotOptimize(dubins_path_object.dubins({0, 0, 0.5}, {800, 0, 2.4})); + benchmark::ClobberMemory(); + } + +} +BENCHMARK(BM_Dubins_a12); + +static void BM_Dubins_a12_random(benchmark::State& state) { + std::random_device rd; + std::mt19937 gen = std::mt19937(rd()); + std::uniform_real_distribution<> disPhi1(0.0, M_PI_2); + std::uniform_real_distribution<> disPhi2(M_PI_2, M_PI); + double max_kappa = 0.01; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + for (auto _ : state){ + benchmark::DoNotOptimize(dubins_path_object.dubins_matrix({0, 0, disPhi1(gen)}, {800, 0, disPhi2(gen)})); + benchmark::ClobberMemory(); + } + +} +BENCHMARK(BM_Dubins_a12_random); + +static void BM_Dubins_a22(benchmark::State& state) { + double max_kappa = 0.01; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + for (auto _ : state){ + benchmark::DoNotOptimize(dubins_path_object.dubins({0, 0, 2.8}, {800, 0, 2.4})); + benchmark::ClobberMemory(); + } + +} +BENCHMARK(BM_Dubins_a22); + +static void BM_Dubins_a22_random(benchmark::State& state) { + std::random_device rd; + std::mt19937 gen = std::mt19937(rd()); + std::uniform_real_distribution<> disPhi2(M_PI_2, M_PI); + double max_kappa = 0.01; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + for (auto _ : state){ + benchmark::DoNotOptimize(dubins_path_object.dubins_matrix({0, 0, disPhi2(gen)}, {800, 0, disPhi2(gen)})); + benchmark::ClobberMemory(); + } + +} +BENCHMARK(BM_Dubins_a22_random); + BENCHMARK_MAIN(); \ No newline at end of file diff --git a/test/unit_test_trochoid_classification.cpp b/test/unit_test_trochoid_classification.cpp index ffa0248..4d123c6 100644 --- a/test/unit_test_trochoid_classification.cpp +++ b/test/unit_test_trochoid_classification.cpp @@ -487,6 +487,90 @@ TEST_F(DubinsTestFixture, failure_case_dubins_four_r1_2) EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5); } +TEST_F(DubinsTestFixture, dubins_a12) +{ + /* + Dubins Matrix Path Length: 1.79769e+308 + Dubins Path Length: 6.60756 + Max_Kappa: 0.00982667 + Start State: -347.852, 202.189, 3.46437 + Goal State: -505.575, 148.801, 0.333584 + Dubins Matrix Path Type: 0, 1, 0 + Dubins Path Type: 2, 0, 2 + */ + max_kappa = 0.01; + // Run dubins on this (should choose dubinsLSL and segfault) + + Dubins::DubinsStateSpace::DubinsState start_state = {0, 0, 0.5}; + Dubins::DubinsStateSpace::DubinsState goal_state = {800, 0, 2.4}; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + dubins_path_matrix = dubins_path_object.dubins_matrix(start_state, goal_state); + dubins_path = dubins_path_object.dubins(start_state, goal_state); + + double dubins_matrix_path_length = dubins_path_matrix.length(); + double dubins_path_length = dubins_path.length(); + + EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5); +} + +TEST_F(DubinsTestFixture, dubins_a22) +{ + /* + Dubins Matrix Path Length: 1.79769e+308 + Dubins Path Length: 6.60756 + Max_Kappa: 0.00982667 + Start State: -347.852, 202.189, 3.46437 + Goal State: -505.575, 148.801, 0.333584 + Dubins Matrix Path Type: 0, 1, 0 + Dubins Path Type: 2, 0, 2 + */ + max_kappa = 0.01; + // Run dubins on this (should choose dubinsLSL and segfault) + + Dubins::DubinsStateSpace::DubinsState start_state = {0, 0, 2.8}; + Dubins::DubinsStateSpace::DubinsState goal_state = {800, 0, 2.4}; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + dubins_path_matrix = dubins_path_object.dubins_matrix(start_state, goal_state); + dubins_path = dubins_path_object.dubins(start_state, goal_state); + + double dubins_matrix_path_length = dubins_path_matrix.length(); + double dubins_path_length = dubins_path.length(); + + EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5); +} + +TEST_F(DubinsTestFixture, dubins_a22_equal_alpha_beta) +{ + /* + Dubins Matrix Path Length: 1.79769e+308 + Dubins Path Length: 6.60756 + Max_Kappa: 0.00982667 + Start State: -347.852, 202.189, 3.46437 + Goal State: -505.575, 148.801, 0.333584 + Dubins Matrix Path Type: 0, 1, 0 + Dubins Path Type: 2, 0, 2 + */ + max_kappa = 0.01; + // Run dubins on this (should choose dubinsLSL and segfault) + + Dubins::DubinsStateSpace::DubinsState start_state = {0, 0, 2.4}; + Dubins::DubinsStateSpace::DubinsState goal_state = {800, 0, 2.4}; + + Dubins::DubinsStateSpace dubins_path_object(1/max_kappa); + + dubins_path_matrix = dubins_path_object.dubins_matrix(start_state, goal_state); + dubins_path = dubins_path_object.dubins(start_state, goal_state); + + double dubins_matrix_path_length = dubins_path_matrix.length(); + double dubins_path_length = dubins_path.length(); + + EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5); +} + TEST_F(DubinsTestFixture, fail_case_8_tests) { max_kappa = 0.066222699999999995; From bfc51a75db58e6442e0c589a46054a29bbfce2a0 Mon Sep 17 00:00:00 2001 From: Brady Moon Date: Tue, 19 Sep 2023 16:22:11 -0400 Subject: [PATCH 6/6] Modify github action to always commit the benchmark results --- .github/workflows/ci-tests.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-tests.yml b/.github/workflows/ci-tests.yml index fd4c018..6daa9b4 100644 --- a/.github/workflows/ci-tests.yml +++ b/.github/workflows/ci-tests.yml @@ -69,8 +69,8 @@ jobs: comment-on-alert: true # Mention @rhysd in the commit comment alert-comment-cc-users: '@bradygm' - alert-threshold: 200% - # comment-always: true + alert-threshold: 150% + comment-always: true # Upload the updated cache file for the next job by actions/cache - name: Run unit tests