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(avoidance): fix build error #4667

Merged
merged 1 commit into from
Aug 18, 2023
Merged
Changes from all commits
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 @@ -129,7 +129,7 @@
return false;
}

return !avoid_data_.target_objects.empty();

Check warning on line 132 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L132

Added line #L132 was not covered by tests
}

bool AvoidanceModule::isExecutionReady() const
Expand Down Expand Up @@ -230,7 +230,7 @@

// sort object order by longitudinal distance
std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) {
return a.longitudinal < b.longitudinal;

Check warning on line 233 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L233

Added line #L233 was not covered by tests
});

// set base path
Expand Down Expand Up @@ -950,7 +950,7 @@
std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3);
al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength(
avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego);
al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose;

Check warning on line 953 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L953

Added line #L953 was not covered by tests
al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position);

al_avoid.end_shift_length = feasible_shift_length.get();
Expand Down Expand Up @@ -1223,7 +1223,7 @@
ret.push_back(calc_gap_shift_line(shift_lines.at(i), shift_lines.at(i + 1)));
}

utils::avoidance::fillAdditionalInfoFromLongitudinal(avoidance_data_, ret);
utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret);

return ret;
}
Expand All @@ -1236,7 +1236,7 @@
return;
}

const auto & data = avoid_data_;

Check warning on line 1239 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L1239

Added line #L1239 was not covered by tests

helper_.alignShiftLinesOrder(shift_lines, false);

Expand Down Expand Up @@ -1695,7 +1695,7 @@
// so that the return-shift will be generated from ego position.
if (!has_candidate_point && !has_registered_point) {
last_sl.end_idx = avoid_data_.ego_closest_path_index;
last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose;

Check warning on line 1698 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L1698

Added line #L1698 was not covered by tests
last_sl.end_shift_length = getCurrentBaseShift();
}
}
Expand Down Expand Up @@ -1737,7 +1737,7 @@
DEBUG_PRINT(
"return shift already exists, but they are all candidates. Add return shift for overwrite.");
last_sl.end_idx = avoid_data_.ego_closest_path_index;
last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose;

Check warning on line 1740 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L1740

Added line #L1740 was not covered by tests
last_sl.end_shift_length = current_base_shift;
}

Expand All @@ -1753,7 +1753,7 @@
const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance;

// If the avoidance point has already been set, the return shift must be set after the point.
const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx);

Check warning on line 1756 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L1756

Added line #L1756 was not covered by tests

// check if there is enough distance for return.
if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0)
Expand Down Expand Up @@ -2108,7 +2108,7 @@

BehaviorModuleOutput AvoidanceModule::plan()
{
const auto & data = avoid_data_;

Check warning on line 2111 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L2111

Added line #L2111 was not covered by tests

resetPathCandidate();
resetPathReference();
Expand Down Expand Up @@ -2166,9 +2166,9 @@
}

if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) {
output.path = std::make_shared<PathWithLaneId>(spline_shift_path.path);

Check warning on line 2169 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L2169

Added line #L2169 was not covered by tests
} else {
output.path = getPreviousModuleOutput().path;

Check warning on line 2171 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L2171

Added line #L2171 was not covered by tests
RCLCPP_WARN(getLogger(), "Previous module lane is updated. Do nothing.");
}

Expand Down Expand Up @@ -2257,7 +2257,7 @@

addNewShiftLines(path_shifter_, shift_lines);

current_raw_shift_lines_ = avoid_data_.unapproved_raw_sl;

Check warning on line 2260 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L2260

Added line #L2260 was not covered by tests

registerRawShiftLines(shift_lines);

Expand Down Expand Up @@ -2333,7 +2333,7 @@
}

const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx)
.point.longitudinal_velocity_mps;

Check warning on line 2336 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L2336

Added line #L2336 was not covered by tests
const double shift_time = PathShifter::calcShiftTimeFromJerk(
front_new_shift_line.getRelativeLength(), helper_.getLateralMaxJerkLimit(),
helper_.getLateralMaxAccelLimit());
Expand Down Expand Up @@ -2905,7 +2905,7 @@
void AvoidanceModule::insertStopPoint(
const bool use_constraints_for_decel, ShiftedPath & shifted_path) const
{
const auto & data = avoid_data_;

Check warning on line 2908 in planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp#L2908

Added line #L2908 was not covered by tests

if (data.safe) {
return;
Expand Down
Loading