diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 55ecee17b9778..854bb34dfe688 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -242,9 +242,8 @@ class AvoidanceModule : public SceneModuleInterface /** * @brief update main avoidance data for avoidance path generation based on latest planner data. - * @return avoidance data. */ - AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const; + void fillFundamentalData(AvoidancePlanningData & data, DebugData & debug); /** * @brief fill additional data so that the module judges target objects. @@ -553,7 +552,7 @@ class AvoidanceModule : public SceneModuleInterface helper::avoidance::AvoidanceHelper helper_; - AvoidancePlanningData avoidance_data_; + AvoidancePlanningData avoid_data_; PathShifter path_shifter_; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 7221840300ac2..65528190c5915 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -116,30 +116,30 @@ bool AvoidanceModule::isExecutionRequested() const DEBUG_PRINT("AVOIDANCE isExecutionRequested"); // Check ego is in preferred lane - updateInfoMarker(avoidance_data_); - updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); + updateInfoMarker(avoid_data_); + updateDebugMarker(avoid_data_, path_shifter_, debug_data_); // there is object that should be avoid. return true. - if (!!avoidance_data_.stop_target_object) { + if (!!avoid_data_.stop_target_object) { return true; } - if (avoidance_data_.unapproved_new_sl.empty()) { + if (avoid_data_.unapproved_new_sl.empty()) { return false; } - return !avoidance_data_.target_objects.empty(); + return !avoid_data_.target_objects.empty(); } bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoidance_data_.safe && avoidance_data_.comfortable; + return avoid_data_.safe && avoid_data_.comfortable; } bool AvoidanceModule::canTransitSuccessState() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; // Change input lane. -> EXIT. if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { @@ -189,10 +189,8 @@ bool AvoidanceModule::canTransitSuccessState() return false; // Keep current state. } -AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const +void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) { - AvoidancePlanningData data; - // reference pose data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_.getPreviousSplineShiftPath()); @@ -224,9 +222,18 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // target objects for avoidance fillAvoidanceTargetObjects(data, debug); - DEBUG_PRINT("target object size = %lu", data.target_objects.size()); + // lost object compensation + utils::avoidance::updateRegisteredObject(registered_objects_, data.target_objects, parameters_); + utils::avoidance::compensateDetectionLost( + registered_objects_, data.target_objects, data.other_objects); - return data; + // 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; + }); + + // set base path + path_shifter_.setPath(data.reference_path); } void AvoidanceModule::fillAvoidanceTargetObjects( @@ -622,7 +629,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif void AvoidanceModule::updateRegisteredRawShiftLines() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; utils::avoidance::fillAdditionalInfoFromPoint(data, registered_raw_shift_lines_); @@ -741,7 +748,7 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) const auto old_size = registered_raw_shift_lines_.size(); auto future_with_info = future; - utils::avoidance::fillAdditionalInfoFromPoint(avoidance_data_, future_with_info); + utils::avoidance::fillAdditionalInfoFromPoint(avoid_data_, future_with_info); printShiftLines(future_with_info, "future_with_info"); printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_"); printShiftLines(current_raw_shift_lines_, "current_raw_shift_lines_"); @@ -936,13 +943,13 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( { const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front; const auto path_front_to_ego = - avoidance_data_.arclength_from_ego.at(avoidance_data_.ego_closest_path_index); + avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); al_avoid.start_longitudinal = std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3); al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( - avoidance_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); - al_avoid.start = avoidance_data_.reference_path.points.at(al_avoid.start_idx).point.pose; + 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; al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); al_avoid.end_shift_length = feasible_shift_length.get(); @@ -1011,8 +1018,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( void AvoidanceModule::generateTotalShiftLine( const AvoidLineArray & avoid_lines, ShiftLineData & shift_line_data) const { - const auto & path = avoidance_data_.reference_path; - const auto & arcs = avoidance_data_.arclength_from_ego; + const auto & path = avoid_data_.reference_path; + const auto & arcs = avoid_data_.arclength_from_ego; const auto N = path.points.size(); auto & sl = shift_line_data; @@ -1061,7 +1068,7 @@ void AvoidanceModule::generateTotalShiftLine( // overwrite shift with current_ego_shift until ego pose. const auto current_shift = helper_.getEgoLinearShift(); - for (size_t i = 0; i <= avoidance_data_.ego_closest_path_index; ++i) { + for (size_t i = 0; i <= avoid_data_.ego_closest_path_index; ++i) { sl.shift_line.at(i) = current_shift; sl.shift_line_grad.at(i) = 0.0; } @@ -1089,7 +1096,7 @@ void AvoidanceModule::generateTotalShiftLine( const auto grad_first_shift_line = (avoid_lines.front().start_shift_length - current_shift) / avoid_lines.front().start_longitudinal; - for (size_t i = avoidance_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { + for (size_t i = avoid_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { sl.shift_line.at(i) = helper_.getLinearShift(getPoint(path.points.at(i))); sl.shift_line_grad.at(i) = grad_first_shift_line; } @@ -1102,8 +1109,8 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ using utils::avoidance::setEndData; using utils::avoidance::setStartData; - const auto & path = avoidance_data_.reference_path; - const auto & arcs = avoidance_data_.arclength_from_ego; + const auto & path = avoid_data_.reference_path; + const auto & arcs = avoid_data_.arclength_from_ego; const auto N = path.points.size(); auto & sl = shift_line_data; @@ -1144,7 +1151,7 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ bool found_first_start = false; constexpr auto CREATE_SHIFT_GRAD_THR = 0.001; constexpr auto IS_ALREADY_SHIFTING_THR = 0.001; - for (size_t i = avoidance_data_.ego_closest_path_index; i < N - 1; ++i) { + for (size_t i = avoid_data_.ego_closest_path_index; i < N - 1; ++i) { const auto & p = path.points.at(i).point.pose; const auto shift = sl.shift_line.at(i); @@ -1196,7 +1203,7 @@ void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const return; } - const auto & data = avoidance_data_; + const auto & data = avoid_data_; helper_.alignShiftLinesOrder(shift_lines, false); @@ -1266,8 +1273,8 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( // debug print { - const auto & arc = avoidance_data_.arclength_from_ego; - const auto & closest = avoidance_data_.ego_closest_path_index; + const auto & arc = avoid_data_.arclength_from_ego; + const auto & closest = avoid_data_.ego_closest_path_index; const auto & sl = shift_line_data.shift_line; const auto & sg = shift_line_data.shift_line_grad; const auto & fg = shift_line_data.forward_grad; @@ -1606,7 +1613,7 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const { constexpr double ep = 1.0e-3; - const auto & data = avoidance_data_; + const auto & data = avoid_data_; const bool has_candidate_point = !sl_candidates.empty(); const bool has_registered_point = !path_shifter_.getShiftLines().empty(); @@ -1654,8 +1661,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // avoidance points: No, shift points: No -> set the ego position to the last shift point // so that the return-shift will be generated from ego position. if (!has_candidate_point && !has_registered_point) { - last_sl.end_idx = avoidance_data_.ego_closest_path_index; - last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; + 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; last_sl.end_shift_length = getCurrentBaseShift(); } } @@ -1696,12 +1703,12 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // set the return-shift from ego. DEBUG_PRINT( "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end_idx = avoidance_data_.ego_closest_path_index; - last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; + 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; last_sl.end_shift_length = current_base_shift; } - const auto & arclength_from_ego = avoidance_data_.arclength_from_ego; + const auto & arclength_from_ego = avoid_data_.arclength_from_ego; const auto nominal_prepare_distance = helper_.getNominalPrepareDistance(); const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(last_sl.end_shift_length); @@ -1713,7 +1720,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) 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 = avoidance_data_.arclength_from_ego.at(last_sl.end_idx); + const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx); // check if there is enough distance for return. if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) @@ -1766,7 +1773,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.start_longitudinal = arclength_from_ego.at(al.start_idx); al.end_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; + al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; @@ -1781,11 +1788,11 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.id = getOriginalShiftLineUniqueId(); al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.start = avoidance_data_.reference_path.points.at(al.start_idx).point.pose; + al.start = avoid_data_.reference_path.points.at(al.start_idx).point.pose; al.start_longitudinal = arclength_from_ego.at(al.start_idx); al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); - al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; + al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = 0.0; al.start_shift_length = last_sl.end_shift_length; @@ -1831,7 +1838,7 @@ bool AvoidanceModule::isSafePath( } const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoidance_data_, planner_data_, parameters_, is_right_shift.value()); + avoid_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { const auto obj_predicted_paths = @@ -1859,7 +1866,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output }; const auto & route_handler = planner_data_->route_handler; - const auto & current_lanes = avoidance_data_.current_lanelets; + const auto & current_lanes = avoid_data_.current_lanelets; const auto & enable_opposite = parameters_->use_opposite_lane; std::vector drivable_lanes; @@ -2000,7 +2007,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output // generate obstacle polygons current_drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; @@ -2068,7 +2075,7 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig BehaviorModuleOutput AvoidanceModule::plan() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; resetPathCandidate(); resetPathReference(); @@ -2116,13 +2123,13 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path.path, parameters_->resample_interval_for_output); } - avoidance_data_.state = updateEgoState(data); + avoid_data_.state = updateEgoState(data); // update output data { updateEgoBehavior(data, spline_shift_path); - updateInfoMarker(avoidance_data_); - updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); + updateInfoMarker(avoid_data_); + updateDebugMarker(avoid_data_, path_shifter_, debug_data_); } if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { @@ -2147,7 +2154,7 @@ BehaviorModuleOutput AvoidanceModule::plan() CandidateOutput AvoidanceModule::planCandidate() const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; CandidateOutput output; @@ -2217,7 +2224,7 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) addNewShiftLines(path_shifter_, shift_lines); - current_raw_shift_lines_ = avoidance_data_.unapproved_raw_sl; + current_raw_shift_lines_ = avoid_data_.unapproved_raw_sl; registerRawShiftLines(shift_lines); @@ -2292,9 +2299,8 @@ void AvoidanceModule::addNewShiftLines( future.push_back(sl); } - const double road_velocity = - avoidance_data_.reference_path.points.at(front_new_shift_line.start_idx) - .point.longitudinal_velocity_mps; + const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx) + .point.longitudinal_velocity_mps; const double shift_time = PathShifter::calcShiftTimeFromJerk( front_new_shift_line.getRelativeLength(), helper_.getLateralMaxJerkLimit(), helper_.getLateralMaxAccelLimit()); @@ -2427,26 +2433,24 @@ void AvoidanceModule::updateData() } debug_data_ = DebugData(); - avoidance_data_ = calcAvoidancePlanningData(debug_data_); + avoid_data_ = AvoidancePlanningData(); - utils::avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, parameters_); - utils::avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); - - std::sort( - avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), - [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); - - path_shifter_.setPath(avoidance_data_.reference_path); + // update base path and target objects. + fillFundamentalData(avoid_data_, debug_data_); // update registered shift point for new reference path & remove past objects updateRegisteredRawShiftLines(); - fillShiftLine(avoidance_data_, debug_data_); - fillEgoStatus(avoidance_data_, debug_data_); - fillDebugData(avoidance_data_, debug_data_); + // update shift line and check path safety. + fillShiftLine(avoid_data_, debug_data_); + + // update ego behavior. + fillEgoStatus(avoid_data_, debug_data_); + + // update debug data. + fillDebugData(avoid_data_, debug_data_); + // update rtc status. updateRTCData(); } @@ -2488,7 +2492,7 @@ void AvoidanceModule::initRTCStatus() void AvoidanceModule::updateRTCData() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path); @@ -2810,7 +2814,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const void AvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (!data.stop_target_object) { return; @@ -2853,7 +2857,7 @@ void AvoidanceModule::insertWaitPoint( void AvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (data.safe) { return; @@ -2901,7 +2905,7 @@ void AvoidanceModule::insertStopPoint( void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const { const auto & p = parameters_; - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (data.target_objects.empty()) { return; @@ -2918,7 +2922,7 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; // do nothing if there is no avoidance target. if (data.target_objects.empty()) {