From 272d0512d1c99637beec517635e129faec8e0069 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 3 Jun 2024 10:29:19 +0900 Subject: [PATCH] fix(staatic_obstacle_avoidance): allow return shift approval even if return shift path is not within drivable area Signed-off-by: satoshi-ota --- .../src/scene.cpp | 49 ++++++++++++------- 1 file changed, 31 insertions(+), 18 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 16effece0c7aa..b7a732359e601 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1257,32 +1257,45 @@ bool AvoidanceModule::isValidShiftLine( } } + const auto is_return_shift = + [](const double start_shift_length, const double end_shift_length, const double threshold) { + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; + }; + // check if the vehicle is in road. (yaw angle is not considered) { const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + parameters_->hard_road_shoulder_margin - parameters_->max_deviation_from_lane; - const size_t start_idx = shift_lines.front().start_idx; - const size_t end_idx = shift_lines.back().end_idx; + for (const auto & shift_line : shift_lines) { + const size_t start_idx = shift_line.start_idx; + const size_t end_idx = shift_line.end_idx; - const auto path = shifter_for_validate.getReferencePath(); - const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); - const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(path.points.at(i)); - lanelet::BasicPoint2d basic_point{p.x, p.y}; - - const auto shift_length = proposed_shift_path.shift_length.at(i); - const auto THRESHOLD = minimum_distance + std::abs(shift_length); + if (is_return_shift( + shift_line.start_shift_length, shift_line.end_shift_length, + parameters_->lateral_small_shift_threshold)) { + continue; + } - if ( - boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < - THRESHOLD) { - RCLCPP_DEBUG_THROTTLE( - getLogger(), *clock_, 1000, - "following latest new shift line may cause deviation from drivable area."); - return false; + const auto path = shifter_for_validate.getReferencePath(); + const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); + const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(path.points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; + + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if ( + boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < + THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } } } }