From 2323c6f08b64660fd0649d806c4360368e3b955c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 15 Feb 2024 20:35:47 +0900 Subject: [PATCH] fix(avoidance): limit drivable lane only when the ego in on original lane (#6349) Signed-off-by: satoshi-ota --- .../src/scene.cpp | 4 +- .../behavior_path_avoidance_module/utils.hpp | 9 ++- .../src/scene.cpp | 48 ++++++++++++--- .../src/utils.cpp | 61 +++++++++++++++---- 4 files changed, 97 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 501d1c157d4df..9f3966e055d9b 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -155,8 +155,8 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( // expand drivable lanes std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( - lanelet, planner_data_, avoidance_parameters_, false)); + data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, avoidance_parameters_)); }); // calc drivable bound diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index da3a224689149..02d8d68cf7a56 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -36,6 +36,9 @@ bool isOnRight(const ObjectData & obj); double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); +bool isWithinLanes( + const lanelet::ConstLanelets & lanelets, std::shared_ptr & planner_data); + bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length); bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length); @@ -157,9 +160,11 @@ std::pair separateObjectsByPath( const std::shared_ptr & parameters, const double object_check_forward_distance, DebugData & debug); -DrivableLanes generateExpandDrivableLanes( +DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet); + +DrivableLanes generateExpandedDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool in_avoidance_maneuver); + const std::shared_ptr & parameters); double calcDistanceToReturnDeadLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index b8253d9aabc0e..75529c35c2af5 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -21,6 +21,7 @@ #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -32,8 +33,6 @@ #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" #include -#include - #include #include #include @@ -223,24 +222,51 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes - const auto has_shift_point = !path_shifter_.getShiftLines().empty(); - const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted(); + const auto is_within_current_lane = + utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_); + const auto red_signal_lane_itr = std::find_if( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet); + return utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_); + }); + const auto not_use_adjacent_lane = + is_within_current_lane && red_signal_lane_itr != data.current_lanelets.end(); + std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( - lanelet, planner_data_, parameters_, in_avoidance_maneuver)); + if (!not_use_adjacent_lane) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + } else if (red_signal_lane_itr->id() != lanelet.id()) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + } else { + data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet)); + } }); // calc drivable bound auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); + const auto use_left_side_hatched_road_marking_area = [&]() { + if (!not_use_adjacent_lane) { + return true; + } + return !planner_data_->route_handler->getRoutingGraphPtr()->left(*red_signal_lane_itr); + }(); + const auto use_right_side_hatched_road_marking_area = [&]() { + if (!not_use_adjacent_lane) { + return true; + } + return !planner_data_->route_handler->getRoutingGraphPtr()->right(*red_signal_lane_itr); + }(); data.left_bound = utils::calcBound( getPreviousModuleOutput().path, planner_data_, shorten_lanes, - parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + use_left_side_hatched_road_marking_area, parameters_->use_intersection_areas, parameters_->use_freespace_areas, true); data.right_bound = utils::calcBound( getPreviousModuleOutput().path, planner_data_, shorten_lanes, - parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + use_right_side_hatched_road_marking_area, parameters_->use_intersection_areas, parameters_->use_freespace_areas, false); // reference path @@ -939,7 +965,11 @@ BehaviorModuleOutput AvoidanceModule::plan() { DrivableAreaInfo current_drivable_area_info; // generate drivable lanes - current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + current_drivable_area_info.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + }); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index e8b7600684509..6a44350dd48c6 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -957,6 +957,44 @@ double calcShiftLength( return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0; } +bool isWithinLanes( + const lanelet::ConstLanelets & lanelets, std::shared_ptr & planner_data) +{ + const auto & rh = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto transform = tier4_autoware_utils::pose2transform(ego_pose); + const auto footprint = tier4_autoware_utils::transformVector( + planner_data->parameters.vehicle_info.createFootprint(), transform); + + lanelet::ConstLanelet closest_lanelet{}; + if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) { + return true; + } + + lanelet::ConstLanelets concat_lanelets{}; + + // push previous lanelet + lanelet::ConstLanelets prev_lanelet; + if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) { + concat_lanelets.push_back(prev_lanelet.front()); + } + + // push nearest lanelet + { + concat_lanelets.push_back(closest_lanelet); + } + + // push next lanelet + lanelet::ConstLanelet next_lanelet; + if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) { + concat_lanelets.push_back(next_lanelet); + } + + const auto combine_lanelet = lanelet::utils::combineLaneletsShape(concat_lanelets); + + return boost::geometry::within(footprint, combine_lanelet.polygon2d().basicPolygon()); +} + bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length) { /** @@ -2076,9 +2114,18 @@ std::pair separateObjectsByPath( return std::make_pair(target_objects, other_objects); } -DrivableLanes generateExpandDrivableLanes( +DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet) +{ + DrivableLanes current_drivable_lanes; + current_drivable_lanes.left_lane = lanelet; + current_drivable_lanes.right_lane = lanelet; + + return current_drivable_lanes; +} + +DrivableLanes generateExpandedDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool in_avoidance_maneuver) + const std::shared_ptr & parameters) { const auto & route_handler = planner_data->route_handler; @@ -2092,11 +2139,6 @@ DrivableLanes generateExpandDrivableLanes( // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto next_lanes = route_handler->getNextLanelets(target_lane); - const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); - if (is_stop_signal && !in_avoidance_maneuver) { - return; - } const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_left_lanelets.empty()) { @@ -2107,11 +2149,6 @@ DrivableLanes generateExpandDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto next_lanes = route_handler->getNextLanelets(target_lane); - const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); - if (is_stop_signal && !in_avoidance_maneuver) { - return; - } const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_right_lanelets.empty()) {