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.

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

fix(start_planner): refine shift pull out path to start pose #5874

Merged
merged 2 commits into from
Dec 20, 2023
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,10 @@ class ShiftPullOut : public PullOutPlannerBase
double calcBeforeShiftedArcLength(
const PathWithLaneId & path, const double target_after_arc_length, const double dr);

bool refineShiftedPathToStartPose(
ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose,
const double longitudinal_acc, const double lateral_acc);

std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;

private:
Expand Down
54 changes: 54 additions & 0 deletions planning/behavior_path_start_planner_module/src/shift_pull_out.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -158,6 +158,58 @@
return std::nullopt;
}

bool ShiftPullOut::refineShiftedPathToStartPose(

Check warning on line 161 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L161

Added line #L161 was not covered by tests
ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose,
const double longitudinal_acc, const double lateral_acc)
{
constexpr double TOLERANCE = 0.01;
constexpr size_t MAX_ITERATION = 100;

// Lambda to check if change is above tolerance
auto is_whithin_tolerance =

Check warning on line 169 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View workflow job for this annotation

GitHub Actions / spell-check-partial

Unknown word (whithin)

Check warning on line 169 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (whithin)
[](const auto & prev_val, const auto & current_val, const auto & tolerance) {
return std::abs(current_val - prev_val) < tolerance;

Check warning on line 171 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L171

Added line #L171 was not covered by tests
};

size_t iteration = 0;
while (iteration < MAX_ITERATION) {

Check warning on line 175 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L175

Added line #L175 was not covered by tests
const double lateral_offset =
motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position);

Check warning on line 177 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L177

Added line #L177 was not covered by tests

PathShifter path_shifter;
path_shifter.setPath(shifted_path.path);
ShiftLine shift_line{};
shift_line.start = start_pose;
shift_line.end = end_pose;
shift_line.end_shift_length = lateral_offset;
path_shifter.addShiftLine(shift_line);
path_shifter.setVelocity(0.0);
path_shifter.setLongitudinalAcceleration(longitudinal_acc);
path_shifter.setLateralAccelerationLimit(lateral_acc);

Check warning on line 188 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L179-L188

Added lines #L179 - L188 were not covered by tests

if (!path_shifter.generate(&shifted_path, false)) {
RCLCPP_WARN(

Check warning on line 191 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L190-L191

Added lines #L190 - L191 were not covered by tests
rclcpp::get_logger("ShiftPullOut:refineShiftedPathToStartPose()"),
"Failed to generate shifted path");
return false;

Check warning on line 194 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L194

Added line #L194 was not covered by tests
}

if (is_whithin_tolerance(

Check warning on line 197 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View workflow job for this annotation

GitHub Actions / spell-check-partial

Unknown word (whithin)

Check warning on line 197 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (whithin)

Check warning on line 197 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L197

Added line #L197 was not covered by tests
lateral_offset,
motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position),

Check warning on line 199 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L199

Added line #L199 was not covered by tests
TOLERANCE)) {
return true;
}

iteration++;
}

Check warning on line 205 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L204-L205

Added lines #L204 - L205 were not covered by tests

RCLCPP_WARN(

Check warning on line 207 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L207

Added line #L207 was not covered by tests
rclcpp::get_logger("ShiftPullOut:refineShiftedPathToStartPose()"),
"Failed to converge lateral offset until max iteration");
return false;
}

Check warning on line 211 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Excess Number of Function Arguments

ShiftPullOut::refineShiftedPathToStartPose has 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
const Pose & start_pose, const Pose & goal_pose)
Expand Down Expand Up @@ -303,6 +355,8 @@
if (!path_shifter.generate(&shifted_path, offset_back)) {
continue;
}
refineShiftedPathToStartPose(

Check warning on line 358 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp#L358

Added line #L358 was not covered by tests
shifted_path, start_pose, *shift_end_pose_ptr, longitudinal_acc, lateral_acc);

Check warning on line 359 in planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

ShiftPullOut::calcPullOutPaths already has high cyclomatic complexity, and now it increases in Lines of Code from 129 to 131. 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.

// set velocity
const size_t pull_out_end_idx =
Expand Down
Loading