From d30236bfe9ce21d9926448ce6222f9b5997a778b Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 23 Jul 2024 18:33:54 +0900 Subject: [PATCH 1/2] refactor(lane_change): separate leading and trailing objects Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene.hpp | 17 +- .../utils/base_class.hpp | 2 +- .../utils/data_structs.hpp | 41 +++- .../utils/debug_structs.hpp | 5 +- .../utils/markers.hpp | 5 +- .../src/interface.cpp | 2 +- .../src/scene.cpp | 182 ++++++++++-------- .../src/utils/markers.cpp | 45 +++-- 8 files changed, 183 insertions(+), 116 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 605e0499adcdb..cc695f820ee38 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -82,7 +82,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( PathSafetyStatus approved_path_safety_status) override; - bool isRequiredStop(const bool is_object_coming_from_rear) override; + bool isRequiredStop(const bool is_trailing_object) override; bool isNearEndOfCurrentLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, @@ -120,19 +120,16 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - ExtendedPredictedObjects getTargetObjects( - const LaneChangeLanesFilteredObjects & predicted_objects, + lane_change::TargetObjects getTargetObjects( + const FilteredByLanesExtendedObjects & predicted_objects, const lanelet::ConstLanelets & current_lanes) const; - LaneChangeLanesFilteredObjects filterObjects() const; + FilteredByLanesExtendedObjects filterObjects() const; void filterOncomingObjects(PredictedObjects & objects) const; - void filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, - std::vector & current_lane_objects, - std::vector & target_lane_objects, - std::vector & other_lane_objects) const; + FilteredByLanesObjects filterObjectsByLanelets( + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double backward_path_length, @@ -170,7 +167,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, - const ExtendedPredictedObjects & collision_check_objects, + const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index c1cc14d98a7c7..ccc9258324ffa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -99,7 +99,7 @@ class LaneChangeBase virtual bool isEgoOnPreparePhase() const = 0; - virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; + virtual bool isRequiredStop(const bool is_trailing_object) = 0; virtual PathSafetyStatus isApprovedPathSafe() const = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 155fbfdb535f9..1c820a566f9d1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -212,11 +212,33 @@ struct Info double terminal_lane_changing_velocity{0.0}; }; +template struct LanesObjects { - ExtendedPredictedObjects current_lane{}; - ExtendedPredictedObjects target_lane{}; - ExtendedPredictedObjects other_lane{}; + Object current_lane{}; + Object target_lane_leading{}; + Object target_lane_trailing{}; + Object other_lane{}; + + LanesObjects() = default; + LanesObjects( + Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane) + : current_lane(std::move(current_lane)), + target_lane_leading(std::move(target_lane_leading)), + target_lane_trailing(std::move(target_lane_trailing)), + other_lane(std::move(other_lane)) + { + } +}; + +struct TargetObjects +{ + ExtendedPredictedObjects leading; + ExtendedPredictedObjects trailing; + TargetObjects(ExtendedPredictedObjects leading, ExtendedPredictedObjects trailing) + : leading(std::move(leading)), trailing(std::move(trailing)) + { + } }; enum class ModuleType { @@ -228,7 +250,13 @@ enum class ModuleType { struct PathSafetyStatus { bool is_safe{true}; - bool is_object_coming_from_rear{false}; + bool is_trailing_object{false}; + + PathSafetyStatus() = default; + PathSafetyStatus(const bool is_safe, const bool is_trailing_object) + : is_safe(is_safe), is_trailing_object(is_trailing_object) + { + } }; struct LanesPolygon @@ -277,12 +305,15 @@ using CommonDataPtr = std::shared_ptr; namespace autoware::behavior_path_planner { +using autoware_perception_msgs::msg::PredictedObject; +using utils::path_safety_checker::ExtendedPredictedObjects; using LaneChangeModuleType = lane_change::ModuleType; using LaneChangeParameters = lane_change::Parameters; using LaneChangeStates = lane_change::States; using LaneChangePhaseInfo = lane_change::PhaseInfo; using LaneChangeInfo = lane_change::Info; -using LaneChangeLanesFilteredObjects = lane_change::LanesObjects; +using FilteredByLanesObjects = lane_change::LanesObjects>; +using FilteredByLanesExtendedObjects = lane_change::LanesObjects; using LateralAccelerationMap = lane_change::LateralAccelerationMap; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp index 1890b9f308e8e..fc51a82a8a842 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -35,7 +35,7 @@ struct Debug LaneChangePaths valid_paths; CollisionCheckDebugMap collision_check_objects; CollisionCheckDebugMap collision_check_objects_after_approval; - LaneChangeLanesFilteredObjects filtered_objects; + FilteredByLanesExtendedObjects filtered_objects; geometry_msgs::msg::Polygon execution_area; geometry_msgs::msg::Pose ego_pose; lanelet::ConstLanelets current_lanes; @@ -55,7 +55,8 @@ struct Debug collision_check_objects.clear(); collision_check_objects_after_approval.clear(); filtered_objects.current_lane.clear(); - filtered_objects.target_lane.clear(); + filtered_objects.target_lane_leading.clear(); + filtered_objects.target_lane_trailing.clear(); filtered_objects.other_lane.clear(); execution_area.points.clear(); current_lanes.clear(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index 91d1f1db15cbc..d403e7e1436c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -29,6 +29,7 @@ namespace marker_utils::lane_change_markers { +using autoware::behavior_path_planner::FilteredByLanesExtendedObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; @@ -39,9 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); MarkerArray showFilteredObjects( - const ExtendedPredictedObjects & current_lane_objects, - const ExtendedPredictedObjects & target_lane_objects, - const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); + const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); MarkerArray createDebugMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index b52679d6778ad..afbfc67caa2f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -307,7 +307,7 @@ bool LaneChangeInterface::canTransitFailureState() return false; } - if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) { + if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { log_debug_throttled("Module require stopping"); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 0e11f38e56db8..082bbc142388a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -465,7 +465,8 @@ void NormalLaneChange::insertStopPoint( // [ego]> | <--- lane change margin ---> [obj]> // ---------------------------------------------------------- const bool has_blocking_target_lane_obj = std::any_of( - target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { + target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), + [&](const auto & o) { const auto v = std::abs(o.initial_twist.twist.linear.x); if (v > lane_change_parameters_->stop_velocity_threshold) { return false; @@ -973,29 +974,30 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( return prepare_segment; } -ExtendedPredictedObjects NormalLaneChange::getTargetObjects( - const LaneChangeLanesFilteredObjects & filtered_objects, +lane_change::TargetObjects NormalLaneChange::getTargetObjects( + const FilteredByLanesExtendedObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const { - ExtendedPredictedObjects target_objects = filtered_objects.target_lane; + ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; const auto is_stuck = isVehicleStuck(current_lanes); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; if (chk_obj_in_curr_lanes || is_stuck) { - target_objects.insert( - target_objects.end(), filtered_objects.current_lane.begin(), + leading_objects.insert( + leading_objects.end(), filtered_objects.current_lane.begin(), filtered_objects.current_lane.end()); } const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; if (chk_obj_in_other_lanes) { - target_objects.insert( - target_objects.end(), filtered_objects.other_lane.begin(), filtered_objects.other_lane.end()); + leading_objects.insert( + leading_objects.end(), filtered_objects.other_lane.begin(), + filtered_objects.other_lane.end()); } - return target_objects; + return {leading_objects, filtered_objects.target_lane_trailing}; } -LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const +FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const { const auto & route_handler = getRouteHandler(); const auto & common_parameters = planner_data_->parameters; @@ -1019,10 +1021,6 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return {}; } - std::vector target_lane_objects; - std::vector current_lane_objects; - std::vector other_lane_objects; - const auto & target_lanes = get_target_lanes(); if (target_lanes.empty()) { @@ -1032,8 +1030,7 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const const auto path = route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - filterObjectsByLanelets( - objects, path, current_lane_objects, target_lane_objects, other_lane_objects); + auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); const auto is_within_vel_th = [](const auto & object) -> bool { constexpr double min_vel_th = 1.0; @@ -1042,14 +1039,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const }; utils::path_safety_checker::filterObjects( - target_lane_objects, [&](const PredictedObject & object) { - const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); - return is_within_vel_th(object) || ahead_of_ego; - }); + filtered_by_lanes_objects.target_lane_trailing, + [&](const PredictedObject & object) { return is_within_vel_th(object); }); if (lane_change_parameters_->check_objects_on_other_lanes) { utils::path_safety_checker::filterObjects( - other_lane_objects, [&](const PredictedObject & object) { + filtered_by_lanes_objects.other_lane, [&](const PredictedObject & object) { const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); return is_within_vel_th(object) && ahead_of_ego; @@ -1057,34 +1052,52 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const } utils::path_safety_checker::filterObjects( - current_lane_objects, [&](const PredictedObject & object) { + filtered_by_lanes_objects.current_lane, [&](const PredictedObject & object) { const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); return is_within_vel_th(object) && ahead_of_ego; }); - LaneChangeLanesFilteredObjects lane_change_target_objects; - + ExtendedPredictedObjects extended_target_lane_leading_objects; const auto is_check_prepare_phase = check_prepare_phase(); - std::for_each(target_lane_objects.begin(), target_lane_objects.end(), [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - lane_change_target_objects.target_lane.push_back(extended_predicted_object); - }); + std::for_each( + filtered_by_lanes_objects.target_lane_leading.begin(), + filtered_by_lanes_objects.target_lane_leading.end(), [&](const auto & object) { + auto extended_predicted_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); + extended_target_lane_leading_objects.push_back(extended_predicted_object); + }); - std::for_each(current_lane_objects.begin(), current_lane_objects.end(), [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - lane_change_target_objects.current_lane.push_back(extended_predicted_object); - }); + ExtendedPredictedObjects extended_target_lane_trailing_objects; + std::for_each( + filtered_by_lanes_objects.target_lane_trailing.begin(), + filtered_by_lanes_objects.target_lane_trailing.end(), [&](const auto & object) { + auto extended_predicted_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); + extended_target_lane_trailing_objects.push_back(extended_predicted_object); + }); - std::for_each(other_lane_objects.begin(), other_lane_objects.end(), [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - lane_change_target_objects.other_lane.push_back(extended_predicted_object); - }); + ExtendedPredictedObjects extended_current_lane_objects; + std::for_each( + filtered_by_lanes_objects.current_lane.begin(), filtered_by_lanes_objects.current_lane.end(), + [&](const auto & object) { + auto extended_predicted_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); + extended_current_lane_objects.push_back(extended_predicted_object); + }); - lane_change_debug_.filtered_objects = lane_change_target_objects; + ExtendedPredictedObjects extended_other_lane_objects; + std::for_each( + filtered_by_lanes_objects.other_lane.begin(), filtered_by_lanes_objects.other_lane.end(), + [&](const auto & object) { + auto extended_predicted_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); + extended_other_lane_objects.push_back(extended_predicted_object); + }); + FilteredByLanesExtendedObjects lane_change_target_objects( + extended_current_lane_objects, extended_target_lane_leading_objects, + extended_target_lane_trailing_objects, extended_other_lane_objects); + lane_change_debug_.filtered_objects = lane_change_target_objects; return lane_change_target_objects; } @@ -1115,12 +1128,14 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -void NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, - std::vector & current_lane_objects, - std::vector & target_lane_objects, - std::vector & other_lane_objects) const +FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const { + std::vector target_lane_leading_objects; + std::vector target_lane_trailing_objects; + std::vector current_lane_objects; + std::vector other_lane_objects; + const auto & current_pose = getEgoPose(); const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; @@ -1152,12 +1167,11 @@ void NormalLaneChange::filterObjectsByLanelets( const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - { - const auto reserve_size = objects.objects.size(); - current_lane_objects.reserve(reserve_size); - target_lane_objects.reserve(reserve_size); - other_lane_objects.reserve(reserve_size); - } + const auto reserve_size = objects.objects.size(); + current_lane_objects.reserve(reserve_size); + target_lane_leading_objects.reserve(reserve_size); + target_lane_trailing_objects.reserve(reserve_size); + other_lane_objects.reserve(reserve_size); for (const auto & object : objects.objects) { const auto is_lateral_far = std::invoke([&]() -> bool { @@ -1176,7 +1190,13 @@ void NormalLaneChange::filterObjectsByLanelets( if ( check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && is_before_terminal()) { - target_lane_objects.push_back(object); + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); + if (ahead_of_ego) { + target_lane_leading_objects.push_back(object); + } else { + target_lane_trailing_objects.push_back(object); + } continue; } @@ -1191,7 +1211,7 @@ void NormalLaneChange::filterObjectsByLanelets( // check if the object intersects with target backward lanes if (is_overlap_target_backward) { - target_lane_objects.push_back(object); + target_lane_trailing_objects.push_back(object); continue; } @@ -1203,6 +1223,10 @@ void NormalLaneChange::filterObjectsByLanelets( other_lane_objects.push_back(object); } + + return { + current_lane_objects, target_lane_leading_objects, target_lane_trailing_objects, + other_lane_objects}; } PathWithLaneId NormalLaneChange::getTargetSegment( @@ -1607,7 +1631,7 @@ bool NormalLaneChange::getLaneChangePaths( if ( !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, *candidate_path, filtered_objects.target_lane, + common_data_ptr_, *candidate_path, filtered_objects.target_lane_leading, lane_change_buffer, lane_change_debug_.collision_check_objects)) { debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " @@ -1620,7 +1644,7 @@ bool NormalLaneChange::getLaneChangePaths( return false; } - const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( + const auto [is_safe, is_trailing_object] = isLaneChangePathSafe( *candidate_path, target_objects, rss_params, lane_change_debug_.collision_check_objects); if (is_safe) { @@ -1798,7 +1822,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects.target_lane, min_lc_length, debug_data); + common_data_ptr_, path, filtered_objects.target_lane_leading, min_lc_length, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -1886,13 +1910,13 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const return true; } -bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) +bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && - isAbleToStopSafely() && is_object_coming_from_rear) { + isAbleToStopSafely() && is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -2042,16 +2066,18 @@ bool NormalLaneChange::calcAbortPath() } PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & collision_check_objects, + const LaneChangePath & lane_change_path, + const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - PathSafetyStatus path_safety_status; + constexpr auto is_safe = true; + constexpr auto is_object_behind_ego = true; - if (collision_check_objects.empty()) { + if (collision_check_objects.leading.empty() && collision_check_objects.trailing.empty()) { RCLCPP_DEBUG(logger_, "There is nothing to check."); - return path_safety_status; + return {is_safe, !is_object_behind_ego}; } const auto & path = lane_change_path.path; @@ -2060,11 +2086,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto current_twist = getEgoTwist(); if (path.points.empty()) { - path_safety_status.is_safe = false; - return path_safety_status; + return {!is_safe, !is_object_behind_ego}; } - const double & time_resolution = lane_change_parameters_->prediction_time_resolution; + const auto time_resolution = lane_change_parameters_->prediction_time_resolution; const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( lane_change_path, current_twist, current_pose, common_parameters, *lane_change_parameters_, @@ -2077,7 +2102,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( constexpr double collision_check_yaw_diff_threshold{M_PI}; - for (const auto & obj : collision_check_objects) { + const auto check_collision = [&](const ExtendedPredictedObject & obj) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); @@ -2110,21 +2135,28 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( continue; } - is_safe = false; - path_safety_status.is_safe = false; utils::path_safety_checker::updateCollisionCheckDebugMap( - debug_data, current_debug_data, is_safe); - const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= - !utils::path_safety_checker::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + debug_data, current_debug_data, !is_safe); + return !is_safe; } utils::path_safety_checker::updateCollisionCheckDebugMap( debug_data, current_debug_data, is_safe); + return is_safe; + }; + + for (const auto & obj : collision_check_objects.trailing) { + if (!check_collision(obj)) { + return {!is_safe, is_object_behind_ego}; + } + } + + for (const auto & obj : collision_check_objects.leading) { + if (!check_collision(obj)) { + return {!is_safe, !is_object_behind_ego}; + } } - return path_safety_status; + return {is_safe, !is_object_behind_ego}; } // Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 85840b2bb3dd6..7aea478bd6794 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -101,27 +101,36 @@ MarkerArray createLaneChangingVirtualWallMarker( } MarkerArray showFilteredObjects( - const ExtendedPredictedObjects & current_lane_objects, - const ExtendedPredictedObjects & target_lane_objects, - const ExtendedPredictedObjects & other_lane_objects, const std::string & ns) + const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns) { int32_t update_id = 0; - auto current_marker = - marker_utils::showFilteredObjects(current_lane_objects, ns, colors::yellow(), update_id); + auto current_marker = marker_utils::showFilteredObjects( + filtered_objects.current_lane, ns, colors::yellow(), update_id); update_id += static_cast(current_marker.markers.size()); - auto target_marker = - marker_utils::showFilteredObjects(target_lane_objects, ns, colors::aqua(), update_id); - update_id += static_cast(target_marker.markers.size()); - auto other_marker = - marker_utils::showFilteredObjects(other_lane_objects, ns, colors::medium_orchid(), update_id); + auto target_leading_marker = marker_utils::showFilteredObjects( + filtered_objects.target_lane_leading, ns, colors::aqua(), update_id); + update_id += static_cast(target_leading_marker.markers.size()); + auto target_trailing_marker = marker_utils::showFilteredObjects( + filtered_objects.target_lane_trailing, ns, colors::blue(), update_id); + update_id += static_cast(target_trailing_marker.markers.size()); + auto other_marker = marker_utils::showFilteredObjects( + filtered_objects.other_lane, ns, colors::medium_orchid(), update_id); MarkerArray marker_array; - marker_array.markers.insert( - marker_array.markers.end(), current_marker.markers.begin(), current_marker.markers.end()); - marker_array.markers.insert( - marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); - marker_array.markers.insert( - marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); + std::move( + current_marker.markers.begin(), current_marker.markers.end(), + std::back_inserter(marker_array.markers)); + std::move( + target_leading_marker.markers.begin(), target_leading_marker.markers.end(), + std::back_inserter(marker_array.markers)); + + std::move( + target_trailing_marker.markers.begin(), target_trailing_marker.markers.end(), + std::back_inserter(marker_array.markers)); + + std::move( + other_marker.markers.begin(), other_marker.markers.end(), + std::back_inserter(marker_array.markers)); return marker_array; } @@ -190,9 +199,7 @@ MarkerArray createDebugMarkerArray( "target_backward_lanes", debug_data.target_backward_lanes, colors::blue(0.2))); add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); - add(showFilteredObjects( - debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, - debug_filtered_objects.other_lane, "object_filtered")); + add(showFilteredObjects(debug_filtered_objects, "object_filtered")); if (!debug_collision_check_object.empty()) { add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info")); From 511886192a68ea1cdb188b5a42ca0a7f7619605e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 9 Aug 2024 16:22:01 +0900 Subject: [PATCH 2/2] Refactor to use common function Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 4 ++ .../src/scene.cpp | 50 +++++-------------- .../src/utils/utils.cpp | 17 +++++++ 3 files changed, 33 insertions(+), 38 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index ec93a3c999152..a270900b491c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -312,6 +312,10 @@ bool is_before_terminal( const PredictedObject & object); double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose); + +ExtendedPredictedObjects transform_to_extended_objects( + const CommonDataPtr & common_data_ptr, const std::vector & objects, + const bool check_prepare_phase); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 082bbc142388a..76368862dd19b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1000,7 +1000,6 @@ lane_change::TargetObjects NormalLaneChange::getTargetObjects( FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const { const auto & route_handler = getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->object_types_to_check); @@ -1057,46 +1056,21 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return is_within_vel_th(object) && ahead_of_ego; }); - ExtendedPredictedObjects extended_target_lane_leading_objects; const auto is_check_prepare_phase = check_prepare_phase(); - std::for_each( - filtered_by_lanes_objects.target_lane_leading.begin(), - filtered_by_lanes_objects.target_lane_leading.end(), [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - extended_target_lane_leading_objects.push_back(extended_predicted_object); - }); - - ExtendedPredictedObjects extended_target_lane_trailing_objects; - std::for_each( - filtered_by_lanes_objects.target_lane_trailing.begin(), - filtered_by_lanes_objects.target_lane_trailing.end(), [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - extended_target_lane_trailing_objects.push_back(extended_predicted_object); - }); - - ExtendedPredictedObjects extended_current_lane_objects; - std::for_each( - filtered_by_lanes_objects.current_lane.begin(), filtered_by_lanes_objects.current_lane.end(), - [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - extended_current_lane_objects.push_back(extended_predicted_object); - }); - - ExtendedPredictedObjects extended_other_lane_objects; - std::for_each( - filtered_by_lanes_objects.other_lane.begin(), filtered_by_lanes_objects.other_lane.end(), - [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - extended_other_lane_objects.push_back(extended_predicted_object); - }); + const auto target_lane_leading_extended_objects = + utils::lane_change::transform_to_extended_objects( + common_data_ptr_, filtered_by_lanes_objects.target_lane_leading, is_check_prepare_phase); + const auto target_lane_trailing_extended_objects = + utils::lane_change::transform_to_extended_objects( + common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing, is_check_prepare_phase); + const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects( + common_data_ptr_, filtered_by_lanes_objects.current_lane, is_check_prepare_phase); + const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects( + common_data_ptr_, filtered_by_lanes_objects.other_lane, is_check_prepare_phase); FilteredByLanesExtendedObjects lane_change_target_objects( - extended_current_lane_objects, extended_target_lane_leading_objects, - extended_target_lane_trailing_objects, extended_other_lane_objects); + current_lane_extended_objects, target_lane_leading_extended_objects, + target_lane_trailing_extended_objects, other_lane_extended_objects); lane_change_debug_.filtered_objects = lane_change_target_objects; return lane_change_target_objects; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 6debfd8cc6172..928506fa448c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -1338,6 +1338,23 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co const auto closest_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, pose.position); return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose)); } + +ExtendedPredictedObjects transform_to_extended_objects( + const CommonDataPtr & common_data_ptr, const std::vector & objects, + const bool check_prepare_phase) +{ + ExtendedPredictedObjects extended_objects; + extended_objects.reserve(objects.size()); + + const auto & bpp_param = *common_data_ptr->bpp_param_ptr; + const auto & lc_param = *common_data_ptr->lc_param_ptr; + std::transform( + objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) { + return utils::lane_change::transform(object, bpp_param, lc_param, check_prepare_phase); + }); + + return extended_objects; +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug