From c2fc98f70a226a327955edac4d4e86e5330a60ab Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 21 Nov 2024 21:06:25 +0900 Subject: [PATCH] todo: define score Signed-off-by: Mamoru Sobue --- .../autoware_test_utils/mock_data_parser.hpp | 3 --- .../examples/plot_map_case1.cpp | 23 ++++++++----------- .../examples/plot_map_case2.cpp | 13 +++++++++-- .../pull_over_planner/bezier_pull_over.cpp | 16 +++++++------ 4 files changed, 30 insertions(+), 25 deletions(-) diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 1732497e31def..5e07237e2c495 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -127,9 +127,6 @@ AccelWithCovariance parse(const YAML::Node & node); template <> AccelWithCovarianceStamped parse(const YAML::Node & node); -template <> -LaneletRoute parse(const YAML::Node & node); - template <> LaneletPrimitive parse(const YAML::Node & node); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp index 15d6744c60032..69240901c4b0b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -337,14 +337,6 @@ std::vector selectPullOverPaths( }), sorted_path_indices.end()); - // compare to sort pull_over_path_candidates based on the order in efficient_path_order - const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { - const auto & order = parameters.efficient_path_order; - const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); - const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); - return a_pos < b_pos; - }; - const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins; const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins; @@ -432,9 +424,6 @@ std::vector selectPullOverPaths( // - both are same hard margin const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; - if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { - return comparePathTypePriority(a, b); - } // otherwise, keep the order. return false; }); @@ -620,12 +609,13 @@ int main(int argc, char ** argv) // plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); + const auto start = std::chrono::steady_clock::now(); std::vector candidates; for (auto i = 0; i < goal_candidates.size(); ++i) { const auto & goal_candidate = goal_candidates.at(i); auto shift_pull_over_planner = autoware::behavior_path_planner::BezierPullOver( *node, goal_planner_parameter, lane_departure_checker); - const auto pull_over_paths = + auto pull_over_paths = shift_pull_over_planner.plans(goal_candidate, 0, planner_data, reference_path); if (!pull_over_paths.empty()) { std::copy( @@ -637,6 +627,13 @@ int main(int argc, char ** argv) const auto filtered_paths = selectPullOverPaths( candidates, goal_candidates, planner_data, goal_planner_parameter, reference_path); std::cout << filtered_paths.size() << std::endl; + const auto end = std::chrono::steady_clock::now(); + std::cout << "computed candidate bezier paths in " + << std::chrono::duration_cast(end - start).count() * 1.0 / + 1000000 + << "msecs" << std::endl; + std::cout << "filtered " << filtered_paths.size() << "/" << candidates.size() << " paths" + << std::endl; /* for (auto i = 0; i < filtered_paths.size(); ++i) { const auto & filtered_path = filtered_paths.at(i); @@ -682,7 +679,7 @@ int main(int argc, char ** argv) footprint, autoware::universe_utils::pose2transform(path_point.point.pose)); plot_footprint(ax2, pose_footprint, "blue"); } - } else if (i % 500 == 0) { + } else if (i % 50 == 0) { std::cout << "plotting " << i << "-th filtered path" << std::endl; plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); plot_path_with_lane_id(ax1, filtered_path.full_path(), color, "", 2.0); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index f966be00c65ed..cdf7b8d13c78e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -604,12 +604,13 @@ int main(int argc, char ** argv) // plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); + const auto start = std::chrono::steady_clock::now(); std::vector candidates; for (auto i = 0; i < goal_candidates.size(); ++i) { const auto & goal_candidate = goal_candidates.at(i); auto shift_pull_over_planner = autoware::behavior_path_planner::BezierPullOver( *node, goal_planner_parameter, lane_departure_checker); - const auto pull_over_paths = + auto pull_over_paths = shift_pull_over_planner.plans(goal_candidate, 0, planner_data, reference_path); if (!pull_over_paths.empty()) { std::copy( @@ -621,6 +622,14 @@ int main(int argc, char ** argv) const auto filtered_paths = selectPullOverPaths( candidates, goal_candidates, planner_data, goal_planner_parameter, reference_path); std::cout << filtered_paths.size() << std::endl; + const auto end = std::chrono::steady_clock::now(); + std::cout << "computed candidate bezier paths in " + << std::chrono::duration_cast(end - start).count() * 1.0 / + 1000000 + << "msecs" << std::endl; + std::cout << "filtered " << filtered_paths.size() << "/" << candidates.size() << " paths" + << std::endl; + /* for (auto i = 0; i < filtered_paths.size(); ++i) { const auto & filtered_path = filtered_paths.at(i); @@ -666,7 +675,7 @@ int main(int argc, char ** argv) footprint, autoware::universe_utils::pose2transform(path_point.point.pose)); plot_footprint(ax2, pose_footprint, "blue"); } - } else if (i % 500 == 0) { + } else if (i % 50 == 0) { std::cout << "plotting " << i << "-th filtered path" << std::endl; plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); plot_path_with_lane_id(ax1, filtered_path.full_path(), color, "", 2.0); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp index 1d9daf099ab3e..256cb486f06aa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -172,13 +172,15 @@ std::vector BezierPullOver::generateBezierPath( -before_shifted_pull_over_distance); std::vector> params; - const size_t n_sample = 5; - for (unsigned i = 0; i < n_sample; ++i) { - for (unsigned j = 0; j < n_sample; j++) { - for (unsigned k = 0; k < n_sample; k++) { - const double v_init_coeff = 1.0 / n_sample * i; - const double v_final_coeff = 1.0 / n_sample * j; - const double acc_coeff = 10.0 / n_sample * k; + const size_t n_sample_v_init = 4; + const size_t n_sample_v_final = 4; + const size_t n_sample_acc = 3; + for (unsigned i = 0; i <= n_sample_v_init; ++i) { + for (unsigned j = 0; j <= n_sample_v_final; j++) { + for (unsigned k = 0; k <= n_sample_acc; k++) { + const double v_init_coeff = i * 0.25; + const double v_final_coeff = j * 0.25; + const double acc_coeff = k * (10.0 / 3); params.emplace_back(v_init_coeff, v_final_coeff, acc_coeff); } }