Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(start_planner): define ignore section separately #6219

Merged
merged 4 commits into from
Jan 31, 2024
Merged

feat(start_planner): define ignore section separately #6219

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4e49a69
Update collision check distances in start planner module
kyoichi-sugahara Jan 30, 2024
50bbff8
Update planning/behavior_path_start_planner_module/src/start_planner_…
kyoichi-sugahara Jan 30, 2024
dea40af
Merge branch 'main' into feat/apply_offset_distance_for_geometric
kyoichi-sugahara Jan 30, 2024
f0e0a3e
Fix collision check distance lookup
kyoichi-sugahara Jan 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions planning/behavior_path_start_planner_module/config/start_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,15 +5,14 @@
th_arrived_distance: 1.0
th_stopped_velocity: 0.01
th_stopped_time: 1.0
collision_check_margins: [2.0, 1.5, 1.0]
collision_check_distance_from_end: 1.0
collision_check_margins: [2.0, 1.0, 0.5, 0.1]
collision_check_margin_from_front_object: 5.0
th_moving_object_velocity: 1.0
th_distance_to_middle_of_the_road: 0.5
center_line_path_interval: 1.0
# shift pull out
enable_shift_pull_out: true
check_shift_path_lane_departure: false
shift_collision_check_distance_from_end: -10.0
minimum_shift_pull_out_distance: 0.0
deceleration_interval: 15.0
lateral_jerk: 0.5
Expand All @@ -23,6 +22,7 @@
maximum_curvature: 0.07
# geometric pull out
enable_geometric_pull_out: true
geometric_collision_check_distance_from_end: 0.0
divide_pull_out_path: true
geometric_pull_out_velocity: 1.0
arc_path_interval: 1.0
Expand All @@ -32,7 +32,7 @@
# search start pose backward
enable_back: true
search_priority: "efficient_path" # "efficient_path" or "short_back_distance"
max_back_distance: 30.0
max_back_distance: 20.0
backward_search_resolution: 2.0
backward_path_update_duration: 3.0
ignore_distance_from_lane_end: 15.0
Expand Down
3 changes: 2 additions & 1 deletion ...ior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,14 +63,14 @@ struct StartPlannerParameters
double intersection_search_length{0.0};
double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
std::vector<double> collision_check_margins{};
double collision_check_distance_from_end{0.0};
double collision_check_margin_from_front_object{0.0};
double th_moving_object_velocity{0.0};
double center_line_path_interval{0.0};

// shift pull out
bool enable_shift_pull_out{false};
bool check_shift_path_lane_departure{false};
double shift_collision_check_distance_from_end{0.0};
double minimum_shift_pull_out_distance{0.0};
int lateral_acceleration_sampling_num{0};
double lateral_jerk{0.0};
Expand All @@ -80,6 +80,7 @@ struct StartPlannerParameters
double deceleration_interval{0.0};
// geometric pull out
bool enable_geometric_pull_out{false};
double geometric_collision_check_distance_from_end;
bool divide_pull_out_path{false};
ParallelParkingParameters parallel_parking_parameters{};
// search start pose backward
Expand Down
3 changes: 2 additions & 1 deletion ..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,8 @@ class StartPlannerModule : public SceneModuleInterface
const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin);

PathWithLaneId extractCollisionCheckSection(const PullOutPath & path);
PathWithLaneId extractCollisionCheckSection(
const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type);
void updateStatusWithCurrentPath(
const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
const behavior_path_planner::PlannerType & planner_type);
Expand Down
6 changes: 4 additions & 2 deletions planning/behavior_path_start_planner_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,17 +44,17 @@
p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
p.collision_check_margins =
node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
p.collision_check_distance_from_end =
node->declare_parameter<double>(ns + "collision_check_distance_from_end");
p.collision_check_margin_from_front_object =
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
// shift pull out
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
p.check_shift_path_lane_departure =
node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
p.shift_collision_check_distance_from_end =
node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");

Check warning on line 57 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/manager.cpp#L56-L57 

Added lines #L56 - L57 were not covered by tests
p.minimum_shift_pull_out_distance =
node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
p.lateral_acceleration_sampling_num =
Expand All @@ -66,6 +66,8 @@
p.deceleration_interval = node->declare_parameter<double>(ns + "deceleration_interval");
// geometric pull out
p.enable_geometric_pull_out = node->declare_parameter<bool>(ns + "enable_geometric_pull_out");
p.geometric_collision_check_distance_from_end =
node->declare_parameter<double>(ns + "geometric_collision_check_distance_from_end");

Check warning on line 70 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

StartPlannerModuleManager::init increases from 264 to 266 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

Check warning on line 70 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/manager.cpp#L69-L70 

Added lines #L69 - L70 were not covered by tests
p.divide_pull_out_path = node->declare_parameter<bool>(ns + "divide_pull_out_path");
p.parallel_parking_parameters.pull_out_velocity =
node->declare_parameter<double>(ns + "geometric_pull_out_velocity");
Expand Down
24 changes: 19 additions & 5 deletions planning/behavior_path_start_planner_module/src/start_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File 

The lines of code increases from 1227 to 1238, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -657,8 +657,8 @@

// check collision
if (utils::checkCollisionBetweenPathFootprintsAndObjects(
vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects,
collision_check_margin)) {
vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()),

Check warning on line 660 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L660 

Added line #L660 was not covered by tests
pull_out_lane_stop_objects, collision_check_margin)) {
return false;
}

Expand All @@ -672,8 +672,17 @@
return true;
}

PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path)
PathWithLaneId StartPlannerModule::extractCollisionCheckSection(

Check warning on line 675 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L675 

Added line #L675 was not covered by tests
const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type)
{
const std::map<PlannerType, double> collision_check_distances = {
{behavior_path_planner::PlannerType::SHIFT,
parameters_->shift_collision_check_distance_from_end},
{behavior_path_planner::PlannerType::GEOMETRIC,
parameters_->geometric_collision_check_distance_from_end}};

Check warning on line 682 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L682 

Added line #L682 was not covered by tests

const double collision_check_distance_from_end = collision_check_distances.at(planner_type);

Check warning on line 684 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L684 

Added line #L684 was not covered by tests

PathWithLaneId combined_path;
for (const auto & partial_path : path.partial_paths) {
combined_path.points.insert(
Expand All @@ -682,7 +691,7 @@
// calculate collision check end idx
const size_t collision_check_end_idx = std::invoke([&]() {
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end);
combined_path.points, path.end_pose.position, collision_check_distance_from_end);

Check warning on line 694 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L694 

Added line #L694 was not covered by tests

if (collision_check_end_pose) {
return motion_utils::findNearestIndex(
Expand Down Expand Up @@ -1403,9 +1412,14 @@
// visualize collision_check_end_pose and footprint
{
const auto local_footprint = vehicle_info_.createFootprint();
std::map<PlannerType, double> collision_check_distances = {
{PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
{PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};

Check warning on line 1417 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1417 

Added line #L1417 was not covered by tests
Comment on lines +1415 to +1417
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(nits: this is duplicated process, maybe it is also good have as member)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ah
define map as member variable you mean?
I agree

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes because this map is like a parameter


double collision_check_distance_from_end = collision_check_distances[status_.planner_type];

Check warning on line 1419 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1419 

Added line #L1419 was not covered by tests
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
getFullPath().points, status_.pull_out_path.end_pose.position,
parameters_->collision_check_distance_from_end);
collision_check_distance_from_end);

Check warning on line 1422 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

StartPlannerModule::setDebugData already has high cyclomatic complexity, and now it increases in Lines of Code from 150 to 154. 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.
if (collision_check_end_pose) {
add(createPoseMarkerArray(
*collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0));
Expand Down
Loading