From baec14ad7d8182bb62f1ae918f998698a1a7a42d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Sep 2023 10:30:39 +0900 Subject: [PATCH 1/2] fix(avoidance): fix candidate path trimming process (#5022) fix(avoidance): fix candidate path visualization Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.cpp | 48 +++++++++---------- 1 file changed, 23 insertions(+), 25 deletions(-) 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 8f2e505fcb76e..41b74485ca04b 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 @@ -2289,37 +2289,35 @@ CandidateOutput AvoidanceModule::planCandidate() const auto shifted_path = data.candidate_path; - if (!data.safe_new_sl.empty()) { // clip from shift start index for visualize - utils::clipPathLength( - shifted_path.path, data.safe_new_sl.front().start_idx, std::numeric_limits::max(), - 0.0); + if (data.safe_new_sl.empty()) { + const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); + utils::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); - const auto sl = helper_.getMainShiftLine(data.safe_new_sl); - const auto sl_front = data.safe_new_sl.front(); - const auto sl_back = data.safe_new_sl.back(); + output.path_candidate = shifted_path.path; + return output; + } - output.lateral_shift = helper_.getRelativeShiftToPath(sl); - output.start_distance_to_path_change = sl_front.start_longitudinal; - output.finish_distance_to_path_change = sl_back.end_longitudinal; + const auto sl = helper_.getMainShiftLine(data.safe_new_sl); + const auto sl_front = data.safe_new_sl.front(); + const auto sl_back = data.safe_new_sl.back(); - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - steering_factor_interface_ptr_->updateSteeringFactor( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, - ""); - } + utils::clipPathLength( + shifted_path.path, sl_front.start_idx, std::numeric_limits::max(), 0.0); - const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); - utils::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); + output.lateral_shift = helper_.getRelativeShiftToPath(sl); + output.start_distance_to_path_change = sl_front.start_longitudinal; + output.finish_distance_to_path_change = sl_back.end_longitudinal; - output.path_candidate = shifted_path.path; + const uint16_t steering_factor_direction = std::invoke([&output]() { + return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + }); + steering_factor_interface_ptr_->updateSteeringFactor( + {sl_front.start, sl_back.end}, + {output.start_distance_to_path_change, output.finish_distance_to_path_change}, + SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, + ""); + output.path_candidate = shifted_path.path; return output; } From 7957ed3eabf840fa8f2bba1ee8de2b6dc7b3bb24 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Sep 2023 12:15:55 +0900 Subject: [PATCH 2/2] fix(avoidance): fix bug in backward length calculation (#5046) * fix(avoidance): calc distance along path Signed-off-by: satoshi-ota * chore(avoidance): remove misreading comment Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../scene_module/avoidance/avoidance_module.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) 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 41b74485ca04b..c0c95771f0654 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 @@ -2127,14 +2127,17 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const { - // special for avoidance: take behind distance upt ot shift-start-point if it exist. + const auto previous_path = helper_.getPreviousReferencePath(); + const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); + max_dist = std::max( + max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition())); } for (const auto & sp : registered_raw_shift_lines_) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), sp.start)); + max_dist = std::max( + max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); } return max_dist; }(); @@ -2142,11 +2145,11 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. const auto backward_length = std::max( planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); - const auto previous_path = helper_.getPreviousReferencePath(); const size_t orig_ego_idx = planner_data_->findEgoIndex(original_path.points); - const size_t prev_ego_idx = - findNearestSegmentIndex(previous_path.points, getPoint(original_path.points.at(orig_ego_idx))); + const size_t prev_ego_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + previous_path.points, getPose(original_path.points.at(orig_ego_idx)), + std::numeric_limits::max(), planner_data_->parameters.ego_nearest_yaw_threshold); size_t clip_idx = 0; for (size_t i = 0; i < prev_ego_idx; ++i) {