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 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}, 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..2257753 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; @@ -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); @@ -271,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; @@ -320,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; @@ -355,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) @@ -373,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) @@ -387,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; @@ -422,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) @@ -436,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; @@ -485,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) { @@ -511,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) { @@ -527,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) { @@ -559,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) { @@ -575,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/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/all_benchmarks.cpp b/test/all_benchmarks.cpp index d02b6ee..af6a7fe 100644 --- a/test/all_benchmarks.cpp +++ b/test/all_benchmarks.cpp @@ -445,5 +445,100 @@ 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); + +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 0c74fa0..4d123c6 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,146 @@ 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, 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; 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);