From 67b4b373ce912a4ab1174111762998f918a2ed3a Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 21 Mar 2024 15:26:21 +0900 Subject: [PATCH] fix(avoidance_by_lane_change): object filtering too conservative Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene.cpp | 26 +++++-------------- .../src/interface.cpp | 2 +- 2 files changed, 7 insertions(+), 21 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 1aab806f76cc9..07c1984f66eb8 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 @@ -81,6 +81,9 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const getCommonParam().vehicle_info, getEgoPose(), std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); + RCLCPP_DEBUG( + logger_, "Conditions ? %f, %f, %f", nearest_object.longitudinal, minimum_lane_change_length, + minimum_avoid_length); return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length); } @@ -128,23 +131,6 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( data.current_lanelets = getCurrentLanes(); - // expand drivable lanes - std::for_each( - data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes( - lanelet, planner_data_, avoidance_parameters_)); - }); - - // calc drivable bound - const auto shorten_lanes = - utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); - data.left_bound = utils::calcBound( - data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true); - data.right_bound = utils::calcBound( - data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false); - - // get related objects from dynamic_objects, and then separates them as target objects and non - // target objects fillAvoidanceTargetObjects(data, debug); return data; @@ -191,9 +177,7 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( [&](const auto & object) { return createObjectData(data, object); }); } - utils::avoidance::filterTargetObjects( - target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance, - planner_data_, p); + data.target_objects = target_lane_objects; } ObjectData AvoidanceByLaneChange::createObjectData( @@ -248,6 +232,8 @@ ObjectData AvoidanceByLaneChange::createObjectData( const auto & vehicle_width = planner_data_->parameters.vehicle_width; utils::avoidance::fillAvoidanceNecessity(object_data, registered_objects_, vehicle_width, p); + utils::avoidance::fillLongitudinalAndLengthByClosestEnvelopeFootprint( + data.reference_path_rough, getEgoPosition(), object_data); return object_data; } diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 0e2aaed2b38f6..1d3be6974cb9c 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -80,13 +80,13 @@ void LaneChangeInterface::updateData() getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); + module_type_->updateSpecialData(); if (isWaitingApproval()) { module_type_->updateLaneChangeStatus(); } updateDebugMarker(); - module_type_->updateSpecialData(); module_type_->resetStopPose(); }