Skip to content

Commit

Permalink
todo: define score
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Dec 13, 2024
1 parent d6b1a47 commit c2fc98f
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 25 deletions.
3 changes: 0 additions & 3 deletions common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
23 changes: 10 additions & 13 deletions ...avior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -337,14 +337,6 @@ std::vector<PullOverPath> 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;

Expand Down Expand Up @@ -432,9 +424,6 @@ std::vector<PullOverPath> selectPullOverPaths(
// - both are same hard margin
const auto & a = pull_over_path_candidates[a_i];
const auto & b = pull_over_path_candidates[b_i];

Check notice on line 426 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

selectPullOverPaths decreases in cyclomatic complexity from 20 to 17, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 426 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Conditional 

selectPullOverPaths decreases from 2 complex conditionals with 4 branches to 1 complex conditionals with 2 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.

Check notice on line 426 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Bumpy Road Ahead 

selectPullOverPaths decreases from 3 to 2 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) {
return comparePathTypePriority(a, b);
}
// otherwise, keep the order.
return false;
});
Expand Down Expand Up @@ -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<PullOverPath> 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(
Expand All @@ -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<std::chrono::nanoseconds>(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);
Expand Down Expand Up @@ -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);
Expand Down
13 changes: 11 additions & 2 deletions ...avior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<PullOverPath> 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(
Expand All @@ -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<std::chrono::nanoseconds>(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);
Expand Down Expand Up @@ -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);
Expand Down
16 changes: 9 additions & 7 deletions ...ner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,13 +172,15 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
-before_shifted_pull_over_distance);

std::vector<std::tuple<double, double, double>> 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);
}
}
Expand Down

0 comments on commit c2fc98f

Please sign in to comment.