From 9b13bdcd8bd17c2b4131416ca36472b13eb7b754 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Tue, 19 Nov 2024 13:15:24 +0900 Subject: [PATCH 01/25] implement function to check if lane change delay is required Signed-off-by: mohammad alqudah --- .../utils/utils.hpp | 5 ++ .../src/utils/utils.cpp | 65 +++++++++++++++++++ 2 files changed, 70 insertions(+) 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 edf64ecdc49ee..3c3aaeee1f836 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 @@ -131,6 +131,11 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); +bool is_delay_lane_change( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const std::vector & target_lane_static_objects, + CollisionCheckDebugMap & object_debug); + bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, CollisionCheckDebugMap & object_debug); 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 69cf6441ef766..53846022d11e4 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 @@ -774,6 +774,71 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } +bool is_delay_lane_change( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const std::vector & target_lane_objects, + CollisionCheckDebugMap & object_debug) +{ + const auto & current_lane_path = common_data_ptr->current_lanes_path; + + if ( + target_lane_objects.empty() || lane_change_path.path.points.empty() || + current_lane_path.points.empty()) { + return false; + } + + const auto vel_threshold = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold; + auto is_static = [&vel_threshold](const ExtendedPredictedObject & obj) { + const auto obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); + return obj_vel_norm < vel_threshold; + }; + + const auto dist_to_end = common_data_ptr->transient_data.dist_to_terminal_end; + const auto dist_buffer = common_data_ptr->transient_data.current_dist_buffer.min; + auto is_near_end = [&dist_to_end, &dist_buffer](const ExtendedPredictedObject & obj) { + const auto dist_obj_to_end = dist_to_end - obj.dist_from_ego; + return dist_obj_to_end <= dist_buffer; + }; + + const auto lc_length = lane_change_path.info.length.lane_changing; + auto is_sufficient_gap = [&lc_length]( + const ExtendedPredictedObject & current_obj, + const ExtendedPredictedObject & next_obj) { + const auto curr_obj_half_length = current_obj.shape.dimensions.x; + const auto next_obj_half_length = next_obj.shape.dimensions.x; + const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; + const auto gap_length = dist_current_to_next - curr_obj_half_length - next_obj_half_length; + return gap_length > lc_length; + }; + + const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; + + auto it = target_lane_objects.begin(); + for (; it < target_lane_objects.end(); ++it) { + if (is_near_end(*it)) break; + + if (!is_static(*it) || it->dist_from_ego < lc_length) continue; + + if ( + delay_lc_param.check_only_parked_vehicle && + !isParkedObject( + lane_change_path.path, *common_data_ptr->route_handler_ptr, *it, + delay_lc_param.min_road_shoulder_width, delay_lc_param.th_parked_vehicle_shift_ratio)) { + continue; + } + + auto next_it = std::next(it); + if (next_it == target_lane_objects.end() || is_sufficient_gap(*it, *next_it)) { + auto debug = utils::path_safety_checker::createObjectDebug(*it); + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return true; + } + } + + return false; +} + bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, CollisionCheckDebugMap & object_debug) From 0705db46728b5f9004b54108f3d7086149ca8893 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Tue, 19 Nov 2024 14:02:51 +0900 Subject: [PATCH 02/25] refactor function isParkedObject Signed-off-by: mohammad alqudah --- .../src/utils/utils.cpp | 55 +++---------------- 1 file changed, 9 insertions(+), 46 deletions(-) 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 53846022d11e4..a19d6f686d219 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 @@ -666,9 +666,6 @@ bool isParkedObject( // +: object position // o: nearest point on centerline - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; - const double object_vel_norm = std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { @@ -687,49 +684,15 @@ bool isParkedObject( const double lat_dist = autoware::motion_utils::calcLateralOffset(path.points, object_pose.position); - lanelet::BasicLineString2d bound; - double center_to_bound_buffer = 0.0; - if (lat_dist > 0.0) { - // left side vehicle - const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet); - const auto most_left_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute most_left_sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - bound = most_left_lanelet.leftBound2d().basicLineString(); - if (most_left_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } else { - // right side vehicle - const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet); - const auto most_right_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages( - most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute most_right_sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - bound = most_right_lanelet.rightBound2d().basicLineString(); - if (most_right_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } + const auto most_side_lanelet = + lat_dist > 0.0 ? route_handler.getMostLeftLanelet(closest_lanelet, false, true) + : route_handler.getMostRightLanelet(closest_lanelet, false, true); + const auto bound = lat_dist > 0.0 ? most_side_lanelet.leftBound2d().basicLineString() + : most_side_lanelet.rightBound2d().basicLineString(); + const lanelet::Attribute lanelet_sub_type = + most_side_lanelet.attribute(lanelet::AttributeName::Subtype); + const auto center_to_bound_buffer = + lanelet_sub_type.value() == "road_shoulder" ? 0.0 : object_check_min_road_shoulder_width; return isParkedObject( closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold); From 500a4f5b3894571a793f66d816762672cd865150 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Tue, 19 Nov 2024 14:29:44 +0900 Subject: [PATCH 03/25] refactor delay lane change parameters Signed-off-by: mohammad alqudah --- .../utils/data_structs.hpp | 15 +++++++++++---- .../src/manager.cpp | 15 +++++++++------ .../src/utils/utils.cpp | 4 ++-- 3 files changed, 22 insertions(+), 12 deletions(-) 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 16b2b69a81af8..8d0569db0a087 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 @@ -100,6 +100,14 @@ struct CancelParameters int deceleration_sampling_num{5}; }; +struct DelayParameters +{ + bool enable{true}; + bool check_only_parked_vehicle{false}; + double min_road_shoulder_width{0.5}; + double th_parked_vehicle_shift_ratio{0.6}; +}; + struct Parameters { // trajectory generation @@ -117,10 +125,6 @@ struct Parameters double lane_change_prepare_duration{4.0}; LateralAccelerationMap lane_change_lat_acc_map; - // parked vehicle - double object_check_min_road_shoulder_width{0.5}; - double object_shiftable_ratio_threshold{0.6}; - // turn signal double min_length_for_turn_signal_activation{10.0}; double length_ratio_for_turn_signal_deactivation{0.8}; @@ -167,6 +171,9 @@ struct Parameters // abort CancelParameters cancel{}; + // delay + DelayParameters delay{}; + // finish judge parameter double lane_change_finish_judge_buffer{3.0}; double finish_judge_lateral_threshold{0.2}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 10dba38645f39..03feb520116aa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -51,12 +51,6 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s p.lateral_acc_sampling_num = getOrDeclareParameter(*node, parameter("lateral_acceleration_sampling_num")); - // parked vehicle detection - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); - p.object_shiftable_ratio_threshold = - getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); - // turn signal p.min_length_for_turn_signal_activation = getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); @@ -229,6 +223,15 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, ns + "pedestrian"); } + // lane change delay + p.delay.enable = getOrDeclareParameter(*node, parameter("delay.enable")); + p.delay.check_only_parked_vehicle = + getOrDeclareParameter(*node, parameter("delay.check_only_parked_vehicle")); + p.delay.min_road_shoulder_width = + getOrDeclareParameter(*node, parameter("delay.min_road_shoulder_width")); + p.delay.th_parked_vehicle_shift_ratio = + getOrDeclareParameter(*node, parameter("delay.th_parked_vehicle_shift_ratio")); + // lane change cancel p.cancel.enable_on_prepare_phase = getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); 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 a19d6f686d219..5d1fa25add26e 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 @@ -809,9 +809,9 @@ bool passed_parked_objects( const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; const auto & object_check_min_road_shoulder_width = - lane_change_parameters.object_check_min_road_shoulder_width; + lane_change_parameters.delay.min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = - lane_change_parameters.object_shiftable_ratio_threshold; + lane_change_parameters.delay.th_parked_vehicle_shift_ratio; const auto & current_lane_path = common_data_ptr->current_lanes_path; if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { From 76f45bec10b3e74be4edf46a1226c3b08c9ba2ce Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Tue, 19 Nov 2024 14:33:47 +0900 Subject: [PATCH 04/25] update lc param yaml Signed-off-by: mohammad alqudah --- .../config/lane_change.param.yaml | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index a2a6fbfd880e4..29ba4e43f0d2b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -15,10 +15,6 @@ longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 - # side walk parked vehicle - object_check_min_road_shoulder_width: 0.5 # [m] - object_shiftable_ratio_threshold: 0.6 - # turn signal min_length_for_turn_signal_activation: 10.0 # [m] length_ratio_for_turn_signal_deactivation: 0.8 # ratio (desired end position) @@ -31,6 +27,13 @@ longitudinal_distance_diff_threshold: prepare: 1.0 lane_changing: 1.0 + + # delay lane change + delay_lane_change: + enable: true + check_only_parked_vehicle: false + min_road_shoulder_width: 0.5 # [m] + th_parked_vehicle_shift_ratio: 0.6 # safety check safety_check: From f38d158bc95b2f3ebe9e4d253f55d3c833df2e9d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Mon, 18 Nov 2024 17:51:12 +0900 Subject: [PATCH 05/25] separate target lane leading objects based on behavior (RT1-8532) Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 3 +- .../utils/data_structs.hpp | 31 ++- .../utils/debug_structs.hpp | 6 +- .../utils/utils.hpp | 21 +- .../package.xml | 1 + .../src/scene.cpp | 218 ++++++------------ .../src/utils/markers.cpp | 68 +++--- .../src/utils/utils.cpp | 135 ++++++----- .../path_safety_checker/objects_filtering.hpp | 5 +- .../path_safety_checker/objects_filtering.cpp | 9 +- .../test/test_objects_filtering.cpp | 15 +- 11 files changed, 237 insertions(+), 275 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 dd21233c716f1..94d40b260c1ce 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 @@ -128,8 +128,7 @@ class NormalLaneChange : public LaneChangeBase void filterOncomingObjects(PredictedObjects & objects) const; - FilteredByLanesObjects filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; + FilteredByLanesExtendedObjects filterObjectsByLanelets(const PredictedObjects & objects) const; bool get_prepare_segment( PathWithLaneId & prepare_segment, const double prepare_length) const override; 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 16b2b69a81af8..ab617cb797f27 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 @@ -262,23 +262,21 @@ struct Info } }; -template +struct TargetLaneLeadingObjects +{ + ExtendedPredictedObjects moving; + ExtendedPredictedObjects stopped; + ExtendedPredictedObjects expanded; + + [[nodiscard]] size_t size() const { return moving.size() + stopped.size() + expanded.size(); } +}; + struct LanesObjects { - 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)) - { - } + ExtendedPredictedObjects others; + ExtendedPredictedObjects current_lane; + ExtendedPredictedObjects target_lane_trailing; + TargetLaneLeadingObjects target_lane_leading; }; struct TargetObjects @@ -418,8 +416,7 @@ using LaneChangeStates = lane_change::States; using LaneChangePhaseInfo = lane_change::PhaseInfo; using LaneChangePhaseMetrics = lane_change::PhaseMetrics; using LaneChangeInfo = lane_change::Info; -using FilteredByLanesObjects = lane_change::LanesObjects>; -using FilteredByLanesExtendedObjects = 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 fc51a82a8a842..d49bf3bac7387 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 @@ -55,9 +55,11 @@ struct Debug collision_check_objects.clear(); collision_check_objects_after_approval.clear(); filtered_objects.current_lane.clear(); - filtered_objects.target_lane_leading.clear(); filtered_objects.target_lane_trailing.clear(); - filtered_objects.other_lane.clear(); + filtered_objects.target_lane_leading.moving.clear(); + filtered_objects.target_lane_leading.stopped.clear(); + filtered_objects.target_lane_leading.expanded.clear(); + filtered_objects.others.clear(); execution_area.points.clear(); current_lanes.clear(); target_lanes.clear(); 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 edf64ecdc49ee..b72db8c735b56 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 @@ -54,6 +54,7 @@ using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; +using behavior_path_planner::lane_change::TargetLaneLeadingObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -144,8 +145,7 @@ lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); ExtendedPredictedObject transform( - const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters); + const PredictedObject & object, const LaneChangeParameters & lane_change_parameters); bool is_collided_polygons_in_lanelet( const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon); @@ -240,17 +240,14 @@ bool is_same_lane_with_prev_iteration( bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object); + const ExtendedPredictedObject & object); bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object); + const ExtendedPredictedObject & 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); - double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, const bool ignore_intersection = false); @@ -294,8 +291,8 @@ double get_min_dist_to_current_lanes_obj( * otherwise, false. */ bool has_blocking_target_object( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, - const double stop_arc_length, const PathWithLaneId & path); + const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length, + const PathWithLaneId & path); /** * @brief Checks if the ego vehicle has passed any turn direction within an intersection. @@ -342,5 +339,11 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & */ bool has_overtaking_turn_lane_object( const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects); + +void filter_target_lane_objects( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, + const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, + const bool before_terminal, TargetLaneLeadingObjects & leading_objects, + ExtendedPredictedObjects & trailing_objects); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index a755b8dc0d42b..aef3d357c20da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -27,6 +27,7 @@ autoware_rtc_interface autoware_universe_utils pluginlib + range-v3 rclcpp tier4_planning_msgs visualization_msgs 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 3f06ff0481c7e..fa79c76525c80 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 @@ -30,6 +30,9 @@ #include #include #include +#include +#include +#include #include #include @@ -45,8 +48,8 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::create_lanes_polygon; -using utils::path_safety_checker::isPolygonOverlapLanelet; namespace calculation = utils::lane_change::calculation; +using utils::path_safety_checker::filter::velocity_filter; NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, @@ -566,7 +569,7 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) // [ego]> | <--- stop margin ---> [obj]> // ---------------------------------------------------------- const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object( - common_data_ptr_, filtered_objects_, stop_arc_length_behind_obj, path); + filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path); if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { set_stop_pose(dist_to_terminal_stop, path); @@ -929,19 +932,21 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( const FilteredByLanesExtendedObjects & filtered_objects, [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { - ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; + ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading.moving; + auto insert_leading_objects = [&](const auto & objects) { + ranges::actions::insert(leading_objects, leading_objects.end(), objects); + }; + + insert_leading_objects(filtered_objects.target_lane_leading.stopped); + insert_leading_objects(filtered_objects.target_lane_leading.expanded); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { - leading_objects.insert( - leading_objects.end(), filtered_objects.current_lane.begin(), - filtered_objects.current_lane.end()); + insert_leading_objects(filtered_objects.current_lane); } const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; if (chk_obj_in_other_lanes) { - leading_objects.insert( - leading_objects.end(), filtered_objects.other_lane.begin(), - filtered_objects.other_lane.end()); + insert_leading_objects(filtered_objects.others); } return {leading_objects, filtered_objects.target_lane_trailing}; @@ -976,52 +981,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto & path = common_data_ptr_->current_lanes_path; - - 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; - constexpr double max_vel_th = std::numeric_limits::max(); - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); - }; - - utils::path_safety_checker::filterObjects( - 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( - 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; - }); - } - - // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior - utils::path_safety_checker::filterObjects( - 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; - }); - - const auto target_lane_leading_extended_objects = - utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_leading); - const auto target_lane_trailing_extended_objects = - utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing); - const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.current_lane); - const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.other_lane); - - FilteredByLanesExtendedObjects lane_change_target_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; + return filterObjectsByLanelets(objects); } void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const @@ -1038,7 +998,8 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const const auto is_stopped_object = [](const auto & object) -> bool { constexpr double min_vel_th = -0.5; constexpr double max_vel_th = 0.5; - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); + return velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, min_vel_th, max_vel_th); }; utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { @@ -1051,120 +1012,79 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const +FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets( + const PredictedObjects & objects) 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_pose = common_data_ptr_->get_ego_pose(); const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto & route_handler = getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto check_optional_polygon = [](const auto & object, const auto & polygon) { - return !polygon.empty() && isPolygonOverlapLanelet(object, polygon); - }; - // get backward lanes - const auto & target_backward_lanes = common_data_ptr_->lanes_ptr->preceding_target; - - { - lane_change_debug_.current_lanes = current_lanes; - lane_change_debug_.target_lanes = target_lanes; - - // TODO(Azu) change the type to std::vector - lane_change_debug_.target_backward_lanes.clear(); - std::for_each( - target_backward_lanes.begin(), target_backward_lanes.end(), - [&](const lanelet::ConstLanelets & target_backward_lane) { - lane_change_debug_.target_backward_lanes.insert( - lane_change_debug_.target_backward_lanes.end(), target_backward_lane.begin(), - target_backward_lane.end()); - }); - } + const auto & current_lanes_ref_path = common_data_ptr_->current_lanes_path; const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); + FilteredByLanesExtendedObjects ext_objects; 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); + ext_objects.current_lane.reserve(reserve_size); + auto & target_lane_leading = ext_objects.target_lane_leading; + target_lane_leading.stopped.reserve(reserve_size); + target_lane_leading.moving.reserve(reserve_size); + target_lane_leading.expanded.reserve(reserve_size); + ext_objects.target_lane_trailing.reserve(reserve_size); + ext_objects.others.reserve(reserve_size); - for (const auto & object : objects.objects) { - const auto is_lateral_far = std::invoke([&]() -> bool { - const auto dist_object_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet( - current_lanes, object.kinematics.initial_pose_with_covariance.pose); - const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; - return std::abs(lateral) > (common_parameters.vehicle_width / 2); - }); + const auto stopped_obj_vel_th = common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; - const auto is_before_terminal = [&]() { - return utils::lane_change::is_before_terminal( - common_data_ptr_, current_lanes_ref_path, object); - }; + for (const auto & object : objects.objects) { + auto ext_object = utils::lane_change::transform(object, *common_data_ptr_->lc_param_ptr); + const auto & ext_obj_pose = ext_object.initial_pose; + ext_object.dist_from_ego = autoware::motion_utils::calcSignedArcLength( + current_lanes_ref_path.points, current_pose.position, ext_obj_pose.position); - if ( - check_optional_polygon(object, lanes_polygon.target) && is_lateral_far && - is_before_terminal()) { - 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; - } + const auto is_before_terminal = + utils::lane_change::is_before_terminal(common_data_ptr_, current_lanes_ref_path, ext_object); - if ( - check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && - is_before_terminal()) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); - const auto stopped_obj_vel_th = - common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < stopped_obj_vel_th) { - if (ahead_of_ego) { - target_lane_leading_objects.push_back(object); - continue; - } - } - } + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, ext_object); - const auto is_overlap_target_backward = std::invoke([&]() -> bool { - const auto check_backward_polygon = [&object](const auto & target_backward_polygon) { - return isPolygonOverlapLanelet(object, target_backward_polygon); - }; - return std::any_of( - lanes_polygon.preceding_target.begin(), lanes_polygon.preceding_target.end(), - check_backward_polygon); - }); + utils::lane_change::filter_target_lane_objects( + common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego, + is_before_terminal, target_lane_leading, ext_objects.target_lane_trailing); - // check if the object intersects with target backward lanes - if (is_overlap_target_backward) { - target_lane_trailing_objects.push_back(object); - continue; - } + // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior + const auto is_moving = velocity_filter( + ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits::max()); - if (check_optional_polygon(object, lanes_polygon.current)) { + if ( + ahead_of_ego && is_moving && is_before_terminal && + !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { // check only the objects that are in front of the ego vehicle - current_lane_objects.push_back(object); + ext_objects.current_lane.push_back(ext_object); continue; } - other_lane_objects.push_back(object); + ext_objects.others.push_back(ext_object); } - return { - current_lane_objects, target_lane_leading_objects, target_lane_trailing_objects, - other_lane_objects}; + const auto dist_comparator = [](const auto & obj1, const auto & obj2) { + return obj1.dist_from_ego < obj2.dist_from_ego; + }; + + // There are no use cases for other lane objects yet, so to save some computation time, we dont + // have to sort them. + ranges::sort(ext_objects.current_lane, dist_comparator); + ranges::sort(target_lane_leading.expanded, dist_comparator); + ranges::sort(target_lane_leading.stopped, dist_comparator); + ranges::sort(target_lane_leading.moving, dist_comparator); + ranges::sort(ext_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) { + return !dist_comparator(obj1, obj2); + }); + + lane_change_debug_.filtered_objects = ext_objects; + + return ext_objects; } PathWithLaneId NormalLaneChange::getTargetSegment( @@ -1420,7 +1340,7 @@ bool NormalLaneChange::check_candidate_path_safety( if ( !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, + common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, lane_change_debug_.collision_check_objects)) { throw std::logic_error( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); @@ -1601,7 +1521,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const } const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); 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 7aea478bd6794..4c1ddb245fa09 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 @@ -104,33 +104,49 @@ MarkerArray showFilteredObjects( const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns) { int32_t update_id = 0; - 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_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; - 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)); + auto reserve_size = filtered_objects.current_lane.size() + filtered_objects.others.size() + + filtered_objects.target_lane_leading.size() + + filtered_objects.target_lane_trailing.size(); + marker_array.markers.reserve(2 * reserve_size); + auto add_objects_to_marker = + [&](const ExtendedPredictedObjects & objects, const ColorRGBA & color) { + auto marker = marker_utils::showFilteredObjects(objects, ns, color, update_id); + update_id += static_cast(marker.markers.size()); + std::move( + marker.markers.begin(), marker.markers.end(), std::back_inserter(marker_array.markers)); + }; + + add_objects_to_marker(filtered_objects.current_lane, colors::yellow()); + add_objects_to_marker(filtered_objects.target_lane_leading.moving, colors::aqua()); + add_objects_to_marker(filtered_objects.target_lane_leading.stopped, colors::light_steel_blue()); + add_objects_to_marker(filtered_objects.target_lane_trailing, colors::blue()); + add_objects_to_marker(filtered_objects.target_lane_leading.expanded, colors::light_pink()); + add_objects_to_marker(filtered_objects.others, colors::medium_orchid()); + + auto add_text = [&](const ExtendedPredictedObjects & objects) { + for (const auto & target_lead_obj : objects) { + auto obj_text = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++update_id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::green()); + obj_text.pose = target_lead_obj.initial_pose; + + std::ostringstream ss; + + ss << "D: " << target_lead_obj.dist_from_ego; + ; + + obj_text.text = ss.str(); + marker_array.markers.push_back(obj_text); + } + }; + + add_text(filtered_objects.current_lane); + add_text(filtered_objects.target_lane_leading.moving); + add_text(filtered_objects.target_lane_leading.expanded); + add_text(filtered_objects.target_lane_leading.stopped); + add_text(filtered_objects.target_lane_trailing); + return marker_array; } 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 69cf6441ef766..a272a089d7a60 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 @@ -25,6 +25,7 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include #include #include #include @@ -33,6 +34,7 @@ #include #include #include +#include #include #include @@ -912,9 +914,7 @@ lanelet::BasicPolygon2d create_polygon( } ExtendedPredictedObject transform( - const PredictedObject & object, - [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters) + const PredictedObject & object, const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object(object); @@ -1078,21 +1078,15 @@ bool is_same_lane_with_prev_iteration( bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object) + const ExtendedPredictedObject & object) { - const auto & current_ego_pose = common_data_ptr->get_ego_pose(); - - const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; - - const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength( - path.points, current_ego_pose.position, obj_position); const auto & ego_info = common_data_ptr->bpp_param_ptr->vehicle_info; const auto lon_dev = std::max( ego_info.max_longitudinal_offset_m + ego_info.rear_overhang_m, object.shape.dimensions.x); // we don't always have to check the distance accurately. - if (std::abs(dist_to_base_link) > lon_dev) { - return dist_to_base_link >= 0.0; + if (std::abs(object.dist_from_ego) > lon_dev) { + return object.dist_from_ego >= 0.0; } const auto & current_footprint = common_data_ptr->transient_data.current_footprint.outer(); @@ -1105,9 +1099,8 @@ bool is_ahead_of_ego( ego_min_dist_to_end = std::min(dist_to_end, ego_min_dist_to_end); } - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); auto current_min_dist_to_end = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon) { + for (const auto & polygon_p : object.initial_polygon.outer()) { const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( path.points, obj_p, path.points.back().point.pose.position); @@ -1118,7 +1111,7 @@ bool is_ahead_of_ego( bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object) + const ExtendedPredictedObject & object) { const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; const auto & lanes_ptr = common_data_ptr->lanes_ptr; @@ -1127,7 +1120,7 @@ bool is_before_terminal( : path.points.back().point.pose.position; double current_max_dist = std::numeric_limits::lowest(); - const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & obj_position = object.initial_pose.position; const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength(path.points, obj_position, terminal_position); // we don't always have to check the distance accurately. @@ -1135,8 +1128,7 @@ bool is_before_terminal( return dist_to_base_link >= 0.0; } - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - for (const auto & polygon_p : obj_polygon) { + for (const auto & polygon_p : object.initial_polygon.outer()) { const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_obj_to_terminal = autoware::motion_utils::calcSignedArcLength(path.points, obj_p, terminal_position); @@ -1156,22 +1148,6 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose)); } -ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects) -{ - 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); - }); - - return extended_objects; -} - double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk, const bool ignore_intersection) @@ -1240,28 +1216,15 @@ double get_min_dist_to_current_lanes_obj( } bool has_blocking_target_object( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, - const double stop_arc_length, const PathWithLaneId & path) + const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length, + const PathWithLaneId & path) { - return std::any_of( - filtered_objects.target_lane_leading.begin(), filtered_objects.target_lane_leading.end(), - [&](const auto & object) { - const auto v = std::abs(object.initial_twist.linear.x); - if (v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { - return false; - } - - // filtered_objects includes objects out of target lanes, so filter them out - if (boost::geometry::disjoint( - object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target)) { - return false; - } - - const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( - path.points, path.points.front().point.pose.position, object.initial_pose.position); - const auto width_margin = object.shape.dimensions.x / 2; - return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; - }); + return ranges::any_of(target_leading_objects.stopped, [&](const auto & object) { + const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, object.initial_pose.position); + const auto width_margin = object.shape.dimensions.x / 2; + return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; + }); } bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr) @@ -1323,4 +1286,66 @@ bool has_overtaking_turn_lane_object( return std::any_of( trailing_objects.begin(), trailing_objects.end(), is_object_overlap_with_target); } + +void filter_target_lane_objects( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, + const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, + const bool before_terminal, TargetLaneLeadingObjects & leading_objects, + ExtendedPredictedObjects & trailing_objects) +{ + using behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m; + const auto & lanes_polygon = *common_data_ptr->lanes_polygon_ptr; + const auto stopped_obj_vel_th = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold; + + const auto is_lateral_far = std::invoke([&]() -> bool { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, object.initial_pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (vehicle_width / 2); + }); + + if (is_lateral_far) { + const auto is_stopped = velocity_filter( + object.initial_twist, -std::numeric_limits::epsilon(), stopped_obj_vel_th); + const auto in_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target); + if (in_target_lanes) { + if (!ahead_of_ego) { + trailing_objects.push_back(object); + return; + } + + if (before_terminal) { + if (is_stopped) { + leading_objects.stopped.push_back(object); + } else { + leading_objects.moving.push_back(object); + } + return; + } + } + + // Check if the object is positioned outside the lane boundary but still close to its edge. + const auto in_expanded_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.expanded_target); + + if (in_expanded_target_lanes && is_stopped && ahead_of_ego) { + leading_objects.expanded.push_back(object); + return; + } + } + + const auto is_overlap_target_backward = + ranges::any_of(lanes_polygon.preceding_target, [&](const auto & target_backward_polygon) { + return !boost::geometry::disjoint(object.initial_polygon, target_backward_polygon); + }); + + // check if the object intersects with target backward lanes + if (is_overlap_target_backward) { + trailing_objects.push_back(object); + return; + } +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index d9873975dabe1..5638915d0b5d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -37,13 +37,12 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters object based on velocity. * - * @param object The predicted object to filter. + * @param twist The predicted object to filter. * @param velocity_threshold Lower bound * @param max_velocity Upper bound * @return Returns true when the object is within a certain velocity range. */ -bool velocity_filter( - const PredictedObject & object, double velocity_threshold, double max_velocity); +bool velocity_filter(const Twist & object, double velocity_threshold, double max_velocity); /** * @brief Filters object based on position. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 80936b655cc48..3c1d9f194aeec 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -26,11 +26,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { -bool velocity_filter(const PredictedObject & object, double velocity_threshold, double max_velocity) +bool velocity_filter(const Twist & object_twist, double velocity_threshold, double max_velocity) { - const auto v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); return (velocity_threshold < v_norm && v_norm < max_velocity); } @@ -148,7 +146,8 @@ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, double velocity_threshold, double max_velocity) { const auto filter = [&](const auto & object) { - return filter::velocity_filter(object, velocity_threshold, max_velocity); + return filter::velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, velocity_threshold, max_velocity); }; auto filtered = objects; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp index a2058db51c9ea..88ed968d27b64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp @@ -66,13 +66,14 @@ TEST(BehaviorPathPlanningObjectsFiltering, velocity_filter) using autoware::behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; PredictedObject predicted_obj; - predicted_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 4.0; - predicted_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 3.0; - - EXPECT_TRUE(velocity_filter(predicted_obj, 4.0, 10.0)); - EXPECT_FALSE(velocity_filter(predicted_obj, 6.0, 10.0)); - EXPECT_FALSE(velocity_filter(predicted_obj, 2.0, 4.9)); - EXPECT_FALSE(velocity_filter(predicted_obj, 6.0, 2.0)); + auto & twist = predicted_obj.kinematics.initial_twist_with_covariance.twist; + twist.linear.x = 4.0; + twist.linear.y = 3.0; + + EXPECT_TRUE(velocity_filter(twist, 4.0, 10.0)); + EXPECT_FALSE(velocity_filter(twist, 6.0, 10.0)); + EXPECT_FALSE(velocity_filter(twist, 2.0, 4.9)); + EXPECT_FALSE(velocity_filter(twist, 6.0, 2.0)); } TEST(BehaviorPathPlanningObjectsFiltering, position_filter) From 806b7c6f1f67f5abe78a5b5a0990c69af6e9721d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 19 Nov 2024 14:45:31 +0900 Subject: [PATCH 06/25] fixed overlapped filtering and lanes debug marker Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 2 +- .../src/scene.cpp | 21 ++++++++++++--- .../src/utils/markers.cpp | 27 +++---------------- .../src/utils/utils.cpp | 12 +++++---- 4 files changed, 30 insertions(+), 32 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 b72db8c735b56..50d70fcd85443 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 @@ -340,7 +340,7 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & bool has_overtaking_turn_lane_object( const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects); -void filter_target_lane_objects( +bool filter_target_lane_objects( const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, const bool before_terminal, TargetLaneLeadingObjects & leading_objects, 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 fa79c76525c80..7f73e2ded4478 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 @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -114,6 +115,18 @@ void NormalLaneChange::update_lanes(const bool is_approved) *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), common_data_ptr_->lc_param_ptr->backward_lane_length); + lane_change_debug_.current_lanes = common_data_ptr_->lanes_ptr->current; + lane_change_debug_.target_lanes = common_data_ptr_->lanes_ptr->target; + + lane_change_debug_.target_backward_lanes.clear(); + ranges::for_each( + common_data_ptr_->lanes_ptr->preceding_target, + [&](const lanelet::ConstLanelets & preceeding_lanes) { + ranges::insert( + lane_change_debug_.target_backward_lanes, lane_change_debug_.target_backward_lanes.end(), + preceeding_lanes); + }); + *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } @@ -1049,9 +1062,11 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets( const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, ext_object); - utils::lane_change::filter_target_lane_objects( - common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego, - is_before_terminal, target_lane_leading, ext_objects.target_lane_trailing); + if (utils::lane_change::filter_target_lane_objects( + common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego, + is_before_terminal, target_lane_leading, ext_objects.target_lane_trailing)) { + continue; + } // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior const auto is_moving = velocity_filter( 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 4c1ddb245fa09..463203b20991f 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 @@ -111,6 +111,10 @@ MarkerArray showFilteredObjects( marker_array.markers.reserve(2 * reserve_size); auto add_objects_to_marker = [&](const ExtendedPredictedObjects & objects, const ColorRGBA & color) { + if (objects.empty()) { + return; + } + auto marker = marker_utils::showFilteredObjects(objects, ns, color, update_id); update_id += static_cast(marker.markers.size()); std::move( @@ -124,29 +128,6 @@ MarkerArray showFilteredObjects( add_objects_to_marker(filtered_objects.target_lane_leading.expanded, colors::light_pink()); add_objects_to_marker(filtered_objects.others, colors::medium_orchid()); - auto add_text = [&](const ExtendedPredictedObjects & objects) { - for (const auto & target_lead_obj : objects) { - auto obj_text = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++update_id, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), colors::green()); - obj_text.pose = target_lead_obj.initial_pose; - - std::ostringstream ss; - - ss << "D: " << target_lead_obj.dist_from_ego; - ; - - obj_text.text = ss.str(); - marker_array.markers.push_back(obj_text); - } - }; - - add_text(filtered_objects.current_lane); - add_text(filtered_objects.target_lane_leading.moving); - add_text(filtered_objects.target_lane_leading.expanded); - add_text(filtered_objects.target_lane_leading.stopped); - add_text(filtered_objects.target_lane_trailing); - return marker_array; } 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 a272a089d7a60..0d38c82105705 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 @@ -1287,7 +1287,7 @@ bool has_overtaking_turn_lane_object( trailing_objects.begin(), trailing_objects.end(), is_object_overlap_with_target); } -void filter_target_lane_objects( +bool filter_target_lane_objects( const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, const bool before_terminal, TargetLaneLeadingObjects & leading_objects, @@ -1314,7 +1314,7 @@ void filter_target_lane_objects( if (in_target_lanes) { if (!ahead_of_ego) { trailing_objects.push_back(object); - return; + return true; } if (before_terminal) { @@ -1323,7 +1323,7 @@ void filter_target_lane_objects( } else { leading_objects.moving.push_back(object); } - return; + return true; } } @@ -1333,7 +1333,7 @@ void filter_target_lane_objects( if (in_expanded_target_lanes && is_stopped && ahead_of_ego) { leading_objects.expanded.push_back(object); - return; + return true; } } @@ -1345,7 +1345,9 @@ void filter_target_lane_objects( // check if the object intersects with target backward lanes if (is_overlap_target_backward) { trailing_objects.push_back(object); - return; + return true; } + + return false; } } // namespace autoware::behavior_path_planner::utils::lane_change From 13d3e880da36b11e66476e1bcefac338d3af919d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 19 Nov 2024 15:59:09 +0900 Subject: [PATCH 07/25] combine filteredObjects function Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 2 - .../src/scene.cpp | 98 ++++++++----------- 2 files changed, 41 insertions(+), 59 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 94d40b260c1ce..87cf08ae8f9a7 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 @@ -128,8 +128,6 @@ class NormalLaneChange : public LaneChangeBase void filterOncomingObjects(PredictedObjects & objects) const; - FilteredByLanesExtendedObjects filterObjectsByLanelets(const PredictedObjects & objects) const; - bool get_prepare_segment( PathWithLaneId & prepare_segment, const double prepare_length) const override; 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 7f73e2ded4478..ee7af41490827 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 @@ -967,7 +967,6 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const { - const auto & route_handler = getRouteHandler(); auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->object_types_to_check); @@ -982,55 +981,12 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto & current_lanes = get_current_lanes(); - - if (current_lanes.empty()) { - return {}; - } - - const auto & target_lanes = get_target_lanes(); - - if (target_lanes.empty()) { + if (!common_data_ptr_->is_lanes_available()) { return {}; } - return filterObjectsByLanelets(objects); -} - -void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const -{ - const auto & current_pose = getEgoPose(); - - const auto is_same_direction = [&](const PredictedObject & object) { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose); - }; - - // Perception noise could make stationary objects seem opposite the ego vehicle; check the - // velocity to prevent this. - const auto is_stopped_object = [](const auto & object) -> bool { - constexpr double min_vel_th = -0.5; - constexpr double max_vel_th = 0.5; - return velocity_filter( - object.kinematics.initial_twist_with_covariance.twist, min_vel_th, max_vel_th); - }; - - utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { - const auto same_direction = is_same_direction(object); - if (same_direction) { - return true; - } - - return is_stopped_object(object); - }); -} - -FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects) const -{ const auto & current_pose = common_data_ptr_->get_ego_pose(); const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - const auto & route_handler = getRouteHandler(); const auto & current_lanes_ref_path = common_data_ptr_->current_lanes_path; @@ -1038,15 +994,15 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets( const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - FilteredByLanesExtendedObjects ext_objects; + FilteredByLanesExtendedObjects filtered_objects; const auto reserve_size = objects.objects.size(); - ext_objects.current_lane.reserve(reserve_size); - auto & target_lane_leading = ext_objects.target_lane_leading; + filtered_objects.current_lane.reserve(reserve_size); + auto & target_lane_leading = filtered_objects.target_lane_leading; target_lane_leading.stopped.reserve(reserve_size); target_lane_leading.moving.reserve(reserve_size); target_lane_leading.expanded.reserve(reserve_size); - ext_objects.target_lane_trailing.reserve(reserve_size); - ext_objects.others.reserve(reserve_size); + filtered_objects.target_lane_trailing.reserve(reserve_size); + filtered_objects.others.reserve(reserve_size); const auto stopped_obj_vel_th = common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; @@ -1064,7 +1020,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets( if (utils::lane_change::filter_target_lane_objects( common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego, - is_before_terminal, target_lane_leading, ext_objects.target_lane_trailing)) { + is_before_terminal, target_lane_leading, filtered_objects.target_lane_trailing)) { continue; } @@ -1076,11 +1032,11 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets( ahead_of_ego && is_moving && is_before_terminal && !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { // check only the objects that are in front of the ego vehicle - ext_objects.current_lane.push_back(ext_object); + filtered_objects.current_lane.push_back(ext_object); continue; } - ext_objects.others.push_back(ext_object); + filtered_objects.others.push_back(ext_object); } const auto dist_comparator = [](const auto & obj1, const auto & obj2) { @@ -1089,17 +1045,45 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets( // There are no use cases for other lane objects yet, so to save some computation time, we dont // have to sort them. - ranges::sort(ext_objects.current_lane, dist_comparator); + ranges::sort(filtered_objects.current_lane, dist_comparator); ranges::sort(target_lane_leading.expanded, dist_comparator); ranges::sort(target_lane_leading.stopped, dist_comparator); ranges::sort(target_lane_leading.moving, dist_comparator); - ranges::sort(ext_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) { + ranges::sort(filtered_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) { return !dist_comparator(obj1, obj2); }); - lane_change_debug_.filtered_objects = ext_objects; + lane_change_debug_.filtered_objects = filtered_objects; + + return filtered_objects; +} + +void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const +{ + const auto & current_pose = getEgoPose(); + + const auto is_same_direction = [&](const PredictedObject & object) { + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose); + }; - return ext_objects; + // Perception noise could make stationary objects seem opposite the ego vehicle; check the + // velocity to prevent this. + const auto is_stopped_object = [](const auto & object) -> bool { + constexpr double min_vel_th = -0.5; + constexpr double max_vel_th = 0.5; + return velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, min_vel_th, max_vel_th); + }; + + utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { + const auto same_direction = is_same_direction(object); + if (same_direction) { + return true; + } + + return is_stopped_object(object); + }); } PathWithLaneId NormalLaneChange::getTargetSegment( From 1ba8643917880ef67ab1e520d54c58415ea96768 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 19 Nov 2024 16:08:53 +0900 Subject: [PATCH 08/25] renaming functions and type Signed-off-by: Zulfaqar Azmi --- .../autoware/behavior_path_lane_change_module/scene.hpp | 4 ++-- .../behavior_path_lane_change_module/utils/base_class.hpp | 2 +- .../utils/data_structs.hpp | 4 ++-- .../utils/debug_structs.hpp | 2 +- .../behavior_path_lane_change_module/utils/markers.hpp | 4 ++-- .../behavior_path_lane_change_module/utils/utils.hpp | 2 +- .../src/scene.cpp | 8 ++++---- .../src/utils/markers.cpp | 2 +- .../src/utils/utils.cpp | 2 +- 9 files changed, 15 insertions(+), 15 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 87cf08ae8f9a7..550925c4a10fc 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 @@ -121,10 +121,10 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_terminal_turn_signal_info() const final; lane_change::TargetObjects get_target_objects( - const FilteredByLanesExtendedObjects & filtered_objects, + const FilteredLanesObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const; - FilteredByLanesExtendedObjects filterObjects() const; + FilteredLanesObjects filter_objects() const; void filterOncomingObjects(PredictedObjects & objects) 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 634f3b737942b..e5c7fcb6d621e 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 @@ -276,7 +276,7 @@ class LaneChangeBase std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; lane_change::CommonDataPtr common_data_ptr_{}; - FilteredByLanesExtendedObjects filtered_objects_{}; + FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; 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 ab617cb797f27..11fad5b49e803 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 @@ -271,7 +271,7 @@ struct TargetLaneLeadingObjects [[nodiscard]] size_t size() const { return moving.size() + stopped.size() + expanded.size(); } }; -struct LanesObjects +struct FilteredLanesObjects { ExtendedPredictedObjects others; ExtendedPredictedObjects current_lane; @@ -416,7 +416,7 @@ using LaneChangeStates = lane_change::States; using LaneChangePhaseInfo = lane_change::PhaseInfo; using LaneChangePhaseMetrics = lane_change::PhaseMetrics; using LaneChangeInfo = lane_change::Info; -using FilteredByLanesExtendedObjects = lane_change::LanesObjects; +using FilteredLanesObjects = lane_change::FilteredLanesObjects; 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 d49bf3bac7387..a9af7323cddec 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; - FilteredByLanesExtendedObjects filtered_objects; + FilteredLanesObjects filtered_objects; geometry_msgs::msg::Polygon execution_area; geometry_msgs::msg::Pose ego_pose; lanelet::ConstLanelets current_lanes; 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 d403e7e1436c7..c95b388a2e4a4 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,7 +29,7 @@ namespace marker_utils::lane_change_markers { -using autoware::behavior_path_planner::FilteredByLanesExtendedObjects; +using autoware::behavior_path_planner::FilteredLanesObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; @@ -40,7 +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 FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns); + const FilteredLanesObjects & 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/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 50d70fcd85443..bd7c0b5c30242 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 @@ -271,7 +271,7 @@ double get_distance_to_next_regulatory_element( * found, returns the maximum possible double value. */ double get_min_dist_to_current_lanes_obj( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects, const double dist_to_target_lane_start, const PathWithLaneId & path); /** 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 ee7af41490827..659f89ac691cd 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 @@ -209,7 +209,7 @@ void NormalLaneChange::update_transient_data() void NormalLaneChange::update_filtered_objects() { - filtered_objects_ = filterObjects(); + filtered_objects_ = filter_objects(); } void NormalLaneChange::updateLaneChangeStatus() @@ -942,7 +942,7 @@ bool NormalLaneChange::get_prepare_segment( } lane_change::TargetObjects NormalLaneChange::get_target_objects( - const FilteredByLanesExtendedObjects & filtered_objects, + const FilteredLanesObjects & filtered_objects, [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading.moving; @@ -965,7 +965,7 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( return {leading_objects, filtered_objects.target_lane_trailing}; } -FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const +FilteredLanesObjects NormalLaneChange::filter_objects() const { auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( @@ -994,7 +994,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - FilteredByLanesExtendedObjects filtered_objects; + FilteredLanesObjects filtered_objects; const auto reserve_size = objects.objects.size(); filtered_objects.current_lane.reserve(reserve_size); auto & target_lane_leading = filtered_objects.target_lane_leading; 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 463203b20991f..83c5e1af92875 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,7 +101,7 @@ MarkerArray createLaneChangingVirtualWallMarker( } MarkerArray showFilteredObjects( - const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns) + const FilteredLanesObjects & filtered_objects, const std::string & ns) { int32_t update_id = 0; MarkerArray marker_array; 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 0d38c82105705..815e6e75bc348 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 @@ -1176,7 +1176,7 @@ double get_distance_to_next_regulatory_element( } double get_min_dist_to_current_lanes_obj( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects, const double dist_to_target_lane_start, const PathWithLaneId & path) { const auto & path_points = path.points; From ca824dee4d35bde900236d2576fa8c62550880e1 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 19 Nov 2024 16:39:43 +0900 Subject: [PATCH 09/25] update some logic to check is stopped Signed-off-by: Zulfaqar Azmi --- .../src/utils/utils.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 815e6e75bc348..79d0dd57372d0 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 @@ -1306,18 +1306,18 @@ bool filter_target_lane_objects( return std::abs(lateral) > (vehicle_width / 2); }); - if (is_lateral_far) { - const auto is_stopped = velocity_filter( - object.initial_twist, -std::numeric_limits::epsilon(), stopped_obj_vel_th); + const auto is_stopped = velocity_filter( + object.initial_twist, -std::numeric_limits::epsilon(), stopped_obj_vel_th); + if (is_lateral_far && before_terminal) { const auto in_target_lanes = !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target); if (in_target_lanes) { - if (!ahead_of_ego) { + if (!ahead_of_ego && !is_stopped) { trailing_objects.push_back(object); return true; } - if (before_terminal) { + if (ahead_of_ego) { if (is_stopped) { leading_objects.stopped.push_back(object); } else { @@ -1343,7 +1343,7 @@ bool filter_target_lane_objects( }); // check if the object intersects with target backward lanes - if (is_overlap_target_backward) { + if (is_overlap_target_backward && !is_stopped) { trailing_objects.push_back(object); return true; } From 50dc4b83eb43d70be1142e1659f514de10fa6d46 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Thu, 21 Nov 2024 17:14:07 +0900 Subject: [PATCH 10/25] rename expanded to stopped_outside_boundary Signed-off-by: Zulfaqar Azmi --- .../utils/data_structs.hpp | 9 +++++++-- .../utils/debug_structs.hpp | 2 +- .../src/scene.cpp | 6 +++--- .../src/utils/markers.cpp | 3 ++- .../src/utils/utils.cpp | 2 +- 5 files changed, 14 insertions(+), 8 deletions(-) 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 11fad5b49e803..7a7841e6177c8 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 @@ -266,9 +266,14 @@ struct TargetLaneLeadingObjects { ExtendedPredictedObjects moving; ExtendedPredictedObjects stopped; - ExtendedPredictedObjects expanded; - [[nodiscard]] size_t size() const { return moving.size() + stopped.size() + expanded.size(); } + // for objects outside of target lanes, but close to its boundaries + ExtendedPredictedObjects stopped_outside_boundary; + + [[nodiscard]] size_t size() const + { + return moving.size() + stopped.size() + stopped_outside_boundary.size(); + } }; struct FilteredLanesObjects 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 a9af7323cddec..fa4ef451e8181 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 @@ -58,7 +58,7 @@ struct Debug filtered_objects.target_lane_trailing.clear(); filtered_objects.target_lane_leading.moving.clear(); filtered_objects.target_lane_leading.stopped.clear(); - filtered_objects.target_lane_leading.expanded.clear(); + filtered_objects.target_lane_leading.stopped_outside_boundary.clear(); filtered_objects.others.clear(); execution_area.points.clear(); current_lanes.clear(); 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 659f89ac691cd..4c39dda01a2d6 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 @@ -951,7 +951,7 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( }; insert_leading_objects(filtered_objects.target_lane_leading.stopped); - insert_leading_objects(filtered_objects.target_lane_leading.expanded); + insert_leading_objects(filtered_objects.target_lane_leading.stopped_outside_boundary); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { insert_leading_objects(filtered_objects.current_lane); @@ -1000,7 +1000,7 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const auto & target_lane_leading = filtered_objects.target_lane_leading; target_lane_leading.stopped.reserve(reserve_size); target_lane_leading.moving.reserve(reserve_size); - target_lane_leading.expanded.reserve(reserve_size); + target_lane_leading.stopped_outside_boundary.reserve(reserve_size); filtered_objects.target_lane_trailing.reserve(reserve_size); filtered_objects.others.reserve(reserve_size); @@ -1046,7 +1046,7 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const // There are no use cases for other lane objects yet, so to save some computation time, we dont // have to sort them. ranges::sort(filtered_objects.current_lane, dist_comparator); - ranges::sort(target_lane_leading.expanded, dist_comparator); + ranges::sort(target_lane_leading.stopped_outside_boundary, dist_comparator); ranges::sort(target_lane_leading.stopped, dist_comparator); ranges::sort(target_lane_leading.moving, dist_comparator); ranges::sort(filtered_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) { 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 83c5e1af92875..0b094c08fc9eb 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 @@ -125,7 +125,8 @@ MarkerArray showFilteredObjects( add_objects_to_marker(filtered_objects.target_lane_leading.moving, colors::aqua()); add_objects_to_marker(filtered_objects.target_lane_leading.stopped, colors::light_steel_blue()); add_objects_to_marker(filtered_objects.target_lane_trailing, colors::blue()); - add_objects_to_marker(filtered_objects.target_lane_leading.expanded, colors::light_pink()); + add_objects_to_marker( + filtered_objects.target_lane_leading.stopped_outside_boundary, colors::light_pink()); add_objects_to_marker(filtered_objects.others, colors::medium_orchid()); return marker_array; 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 79d0dd57372d0..830e2bc5be18c 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 @@ -1332,7 +1332,7 @@ bool filter_target_lane_objects( !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.expanded_target); if (in_expanded_target_lanes && is_stopped && ahead_of_ego) { - leading_objects.expanded.push_back(object); + leading_objects.stopped_outside_boundary.push_back(object); return true; } } From c99d4f2a5bc01e65e82ab0ef1ce8ee677f1a3444 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Thu, 21 Nov 2024 17:31:53 +0900 Subject: [PATCH 11/25] Include docstring Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) 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 bd7c0b5c30242..a02a61d5e72b6 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 @@ -340,6 +340,29 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & bool has_overtaking_turn_lane_object( const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects); +/** + * @brief Filters objects based on their positions and velocities relative to the ego vehicle and + * the target lane. + * + * This function evaluates whether an object should be classified as a leading or trailing object + * in the context of a lane change. Objects are filtered based on their lateral distance from + * the ego vehicle, velocity, and whether they are within the target lane or its expanded + * boundaries. + * + * @param common_data_ptr Shared pointer to CommonData containing information about current lanes, + * vehicle dimensions, lane polygons, and behavior parameters. + * @param object An extended predicted object representing a potential obstacle in the environment. + * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the + * current lanes. + * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle. + * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of + * the lane. + * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or + * outside boundaries). + * @param trailing_objects Reference to a collection for storing trailing objects. + * + * @return true if the object is classified as either leading or trailing, false otherwise. + */ bool filter_target_lane_objects( const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, From 816193b9d46fa56b568afc544bf0bf15b81afeb4 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 22 Nov 2024 09:12:28 +0900 Subject: [PATCH 12/25] =?UTF-8?q?rename=20stopped=5Foutside=5Fboundary=20?= =?UTF-8?q?=E2=86=92=20stopped=5Fat=5Fbound?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Zulfaqar Azmi --- .../behavior_path_lane_change_module/utils/data_structs.hpp | 4 ++-- .../utils/debug_structs.hpp | 2 +- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 6 +++--- .../src/utils/markers.cpp | 2 +- .../src/utils/utils.cpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) 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 7a7841e6177c8..231353e812dd1 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 @@ -268,11 +268,11 @@ struct TargetLaneLeadingObjects ExtendedPredictedObjects stopped; // for objects outside of target lanes, but close to its boundaries - ExtendedPredictedObjects stopped_outside_boundary; + ExtendedPredictedObjects stopped_at_bound; [[nodiscard]] size_t size() const { - return moving.size() + stopped.size() + stopped_outside_boundary.size(); + return moving.size() + stopped.size() + stopped_at_bound.size(); } }; 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 fa4ef451e8181..a28b8523a75c7 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 @@ -58,7 +58,7 @@ struct Debug filtered_objects.target_lane_trailing.clear(); filtered_objects.target_lane_leading.moving.clear(); filtered_objects.target_lane_leading.stopped.clear(); - filtered_objects.target_lane_leading.stopped_outside_boundary.clear(); + filtered_objects.target_lane_leading.stopped_at_bound.clear(); filtered_objects.others.clear(); execution_area.points.clear(); current_lanes.clear(); 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 4c39dda01a2d6..5dd883ad08a76 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 @@ -951,7 +951,7 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( }; insert_leading_objects(filtered_objects.target_lane_leading.stopped); - insert_leading_objects(filtered_objects.target_lane_leading.stopped_outside_boundary); + insert_leading_objects(filtered_objects.target_lane_leading.stopped_at_bound); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { insert_leading_objects(filtered_objects.current_lane); @@ -1000,7 +1000,7 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const auto & target_lane_leading = filtered_objects.target_lane_leading; target_lane_leading.stopped.reserve(reserve_size); target_lane_leading.moving.reserve(reserve_size); - target_lane_leading.stopped_outside_boundary.reserve(reserve_size); + target_lane_leading.stopped_at_bound.reserve(reserve_size); filtered_objects.target_lane_trailing.reserve(reserve_size); filtered_objects.others.reserve(reserve_size); @@ -1046,7 +1046,7 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const // There are no use cases for other lane objects yet, so to save some computation time, we dont // have to sort them. ranges::sort(filtered_objects.current_lane, dist_comparator); - ranges::sort(target_lane_leading.stopped_outside_boundary, dist_comparator); + ranges::sort(target_lane_leading.stopped_at_bound, dist_comparator); ranges::sort(target_lane_leading.stopped, dist_comparator); ranges::sort(target_lane_leading.moving, dist_comparator); ranges::sort(filtered_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) { 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 0b094c08fc9eb..5700842780395 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 @@ -126,7 +126,7 @@ MarkerArray showFilteredObjects( add_objects_to_marker(filtered_objects.target_lane_leading.stopped, colors::light_steel_blue()); add_objects_to_marker(filtered_objects.target_lane_trailing, colors::blue()); add_objects_to_marker( - filtered_objects.target_lane_leading.stopped_outside_boundary, colors::light_pink()); + filtered_objects.target_lane_leading.stopped_at_bound, colors::light_pink()); add_objects_to_marker(filtered_objects.others, colors::medium_orchid()); return marker_array; 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 830e2bc5be18c..49551aa73f635 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 @@ -1332,7 +1332,7 @@ bool filter_target_lane_objects( !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.expanded_target); if (in_expanded_target_lanes && is_stopped && ahead_of_ego) { - leading_objects.stopped_outside_boundary.push_back(object); + leading_objects.stopped_at_bound.push_back(object); return true; } } From 3d969677decf12fdaeca15016ac9c82e8b1fdba0 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 22 Nov 2024 10:35:13 +0900 Subject: [PATCH 13/25] Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi --- .../utils/path_safety_checker/objects_filtering.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 5638915d0b5d6..a8143bc6b8129 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -42,7 +42,7 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; * @param max_velocity Upper bound * @return Returns true when the object is within a certain velocity range. */ -bool velocity_filter(const Twist & object, double velocity_threshold, double max_velocity); +bool velocity_filter(const Twist & object_twist, double velocity_threshold, double max_velocity); /** * @brief Filters object based on position. From 1e209b4d1a7a302563dd4fa74a4bd26a05b89ef1 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 22 Nov 2024 10:35:38 +0900 Subject: [PATCH 14/25] Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi --- .../utils/path_safety_checker/objects_filtering.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index a8143bc6b8129..5c29f347ad9ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -37,7 +37,7 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters object based on velocity. * - * @param twist The predicted object to filter. + * @param twist The twist of predicted object to filter. * @param velocity_threshold Lower bound * @param max_velocity Upper bound * @return Returns true when the object is within a certain velocity range. From be8413acfa9629fbb4065a3c3902b29a505b35d5 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 22 Nov 2024 10:47:51 +0900 Subject: [PATCH 15/25] spell-check Signed-off-by: Zulfaqar Azmi --- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 5dd883ad08a76..942902b836c95 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 @@ -121,10 +121,10 @@ void NormalLaneChange::update_lanes(const bool is_approved) lane_change_debug_.target_backward_lanes.clear(); ranges::for_each( common_data_ptr_->lanes_ptr->preceding_target, - [&](const lanelet::ConstLanelets & preceeding_lanes) { + [&](const lanelet::ConstLanelets & preceding_lanes) { ranges::insert( lane_change_debug_.target_backward_lanes, lane_change_debug_.target_backward_lanes.end(), - preceeding_lanes); + preceding_lanes); }); *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); From 9e977347a40b9fcb2d5286f2942709f2fc20e284 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 22 Nov 2024 16:16:41 +0900 Subject: [PATCH 16/25] add docstring for function is_delay_lane_change Signed-off-by: mohammad alqudah --- .../config/lane_change.param.yaml | 2 +- .../utils/utils.hpp | 21 ++++++++++++++- .../src/manager.cpp | 10 +++---- .../src/scene.cpp | 8 +++--- .../src/utils/utils.cpp | 26 ++++++++----------- 5 files changed, 40 insertions(+), 27 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 29ba4e43f0d2b..a4a42fdf2300e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -27,7 +27,7 @@ longitudinal_distance_diff_threshold: prepare: 1.0 lane_changing: 1.0 - + # delay lane change delay_lane_change: enable: true 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 982de0c05608e..fc60fde4ac740 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 @@ -132,9 +132,28 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); +/** + * @brief Checks if delaying of lane change maneuver is necessary + * + * @details Scans through the provided target objects (assumed to be ordered from closest to + * furthest), and returns true if any of the objects satisfy the following conditions: + * - Not near the end of current lanes + * - There is sufficient distance from object to next one to do lane change + * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects + * which pass isParkedObject() check will be considered. + * + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, + * and transient data. + * @param lane_change_path Cadidate lane change path to apply checks on. + * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include + * target lane leading static objects). + * @param object_debug Collision check debug struct to be updated if any of the traget objects + * satisfy the conditions. + * @return bool True if conditions to delay lane change are met + */ bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & target_lane_static_objects, + const std::vector & target_objects, CollisionCheckDebugMap & object_debug); bool passed_parked_objects( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 03feb520116aa..f4114dd5bc9c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -224,13 +224,13 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lane change delay - p.delay.enable = getOrDeclareParameter(*node, parameter("delay.enable")); + p.delay.enable = getOrDeclareParameter(*node, parameter("delay_lane_change.enable")); p.delay.check_only_parked_vehicle = - getOrDeclareParameter(*node, parameter("delay.check_only_parked_vehicle")); + getOrDeclareParameter(*node, parameter("delay_lane_change.check_only_parked_vehicle")); p.delay.min_road_shoulder_width = - getOrDeclareParameter(*node, parameter("delay.min_road_shoulder_width")); - p.delay.th_parked_vehicle_shift_ratio = - getOrDeclareParameter(*node, parameter("delay.th_parked_vehicle_shift_ratio")); + getOrDeclareParameter(*node, parameter("delay_lane_change.min_road_shoulder_width")); + p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( + *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); // lane change cancel p.cancel.enable_on_prepare_phase = 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 942902b836c95..5360d76c9941c 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 @@ -1338,7 +1338,7 @@ bool NormalLaneChange::check_candidate_path_safety( } if ( - !is_stuck && !utils::lane_change::passed_parked_objects( + !is_stuck && utils::lane_change::is_delay_lane_change( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, lane_change_debug_.collision_check_objects)) { throw std::logic_error( @@ -1519,10 +1519,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {false, true}; } - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data); - - if (!has_passed_parked_objects) { + if (utils::lane_change::is_delay_lane_change( + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data)) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); return {false, false}; } 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 41d1d678d5780..f4f087b031c79 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 @@ -741,23 +741,17 @@ bool isParkedObject( bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & target_lane_objects, + const std::vector & target_objects, CollisionCheckDebugMap & object_debug) { const auto & current_lane_path = common_data_ptr->current_lanes_path; if ( - target_lane_objects.empty() || lane_change_path.path.points.empty() || + target_objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { return false; } - const auto vel_threshold = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold; - auto is_static = [&vel_threshold](const ExtendedPredictedObject & obj) { - const auto obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); - return obj_vel_norm < vel_threshold; - }; - const auto dist_to_end = common_data_ptr->transient_data.dist_to_terminal_end; const auto dist_buffer = common_data_ptr->transient_data.current_dist_buffer.min; auto is_near_end = [&dist_to_end, &dist_buffer](const ExtendedPredictedObject & obj) { @@ -765,24 +759,26 @@ bool is_delay_lane_change( return dist_obj_to_end <= dist_buffer; }; - const auto lc_length = lane_change_path.info.length.lane_changing; - auto is_sufficient_gap = [&lc_length]( + const auto ego_vel = common_data_ptr->get_ego_speed(); + const auto min_lon_acc = common_data_ptr->lc_param_ptr->min_longitudinal_acc; + const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc)); + auto is_sufficient_gap = [&gap_threshold]( const ExtendedPredictedObject & current_obj, const ExtendedPredictedObject & next_obj) { const auto curr_obj_half_length = current_obj.shape.dimensions.x; const auto next_obj_half_length = next_obj.shape.dimensions.x; const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; const auto gap_length = dist_current_to_next - curr_obj_half_length - next_obj_half_length; - return gap_length > lc_length; + return gap_length > gap_threshold; }; const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; - auto it = target_lane_objects.begin(); - for (; it < target_lane_objects.end(); ++it) { + auto it = target_objects.begin(); + for (; it < target_objects.end(); ++it) { if (is_near_end(*it)) break; - if (!is_static(*it) || it->dist_from_ego < lc_length) continue; + if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue; if ( delay_lc_param.check_only_parked_vehicle && @@ -793,7 +789,7 @@ bool is_delay_lane_change( } auto next_it = std::next(it); - if (next_it == target_lane_objects.end() || is_sufficient_gap(*it, *next_it)) { + if (next_it == target_objects.end() || is_sufficient_gap(*it, *next_it)) { auto debug = utils::path_safety_checker::createObjectDebug(*it); debug.second.unsafe_reason = "delay lane change"; utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); From 2dc863a4cf5cd30f5eb9b1aea962ec0d5f35d991 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 25 Nov 2024 16:33:37 +0900 Subject: [PATCH 17/25] remove unused functions Signed-off-by: mohammad alqudah --- .../utils/utils.hpp | 9 -- .../src/utils/utils.cpp | 126 ------------------ 2 files changed, 135 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 fc60fde4ac740..c57b5b8a37e66 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 @@ -156,15 +156,6 @@ bool is_delay_lane_change( const std::vector & target_objects, CollisionCheckDebugMap & object_debug); -bool passed_parked_objects( - const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug); - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); - lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); 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 f4f087b031c79..df09e35108c5b 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 @@ -800,132 +800,6 @@ bool is_delay_lane_change( return false; } -bool passed_parked_objects( - const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug) -{ - const auto route_handler = *common_data_ptr->route_handler_ptr; - const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; - const auto & object_check_min_road_shoulder_width = - lane_change_parameters.delay.min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = - lane_change_parameters.delay.th_parked_vehicle_shift_ratio; - const auto & current_lane_path = common_data_ptr->current_lanes_path; - - if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { - return true; - } - - const auto leading_obj_idx = getLeadingStaticObjectIdx( - route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold); - if (!leading_obj_idx) { - return true; - } - - const auto & leading_obj = objects.at(*leading_obj_idx); - auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); - const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); - if (leading_obj_poly.outer().empty()) { - return true; - } - - const auto & current_path_end = current_lane_path.points.back().point.pose.position; - const auto dist_to_path_end = [&](const auto & src_point) { - if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { - const auto goal_pose = route_handler.getGoalPose(); - return motion_utils::calcSignedArcLength( - current_lane_path.points, src_point, goal_pose.position); - } - return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); - }; - - const auto min_dist_to_end_of_current_lane = std::invoke([&]() { - auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); - for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); - const auto dist = dist_to_path_end(obj_p); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - } - return min_dist_to_end_of_current_lane; - }); - - // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { - return true; - } - - const auto current_pose = common_data_ptr->get_ego_pose(); - const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); - - if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { - return true; - } - - debug.second.unsafe_reason = "delay lane change"; - utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return false; -} - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) -{ - const auto & path = lane_change_path.path; - - if (path.points.empty() || objects.empty()) { - return {}; - } - - const auto & lane_change_start = lane_change_path.info.lane_changing_start; - const auto & path_end = path.points.back(); - - double dist_lc_start_to_leading_obj = 0.0; - std::optional leading_obj_idx = std::nullopt; - for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { - const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose; - - // ignore non-static object - // TODO(shimizu): parametrize threshold - const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); - if (obj_vel_norm > 1.0) { - continue; - } - - // ignore non-parked object - if (!isParkedObject( - path, route_handler, obj, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold)) { - continue; - } - - const double dist_back_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, path_end.point.pose.position, obj_pose.position); - if (dist_back_to_obj > 0.0) { - // object is not on the lane change path - continue; - } - - const double dist_lc_start_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, lane_change_start.position, obj_pose.position); - if (dist_lc_start_to_obj < 0.0) { - // object is on the lane changing path or behind it. It will be detected in safety check - continue; - } - - if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) { - dist_lc_start_to_leading_obj = dist_lc_start_to_obj; - leading_obj_idx = obj_idx; - } - } - - return leading_obj_idx; -} - lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) { From 236d01f5ebb92a8a6b631d97bb94901994a7f3fe Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 27 Nov 2024 10:49:48 +0900 Subject: [PATCH 18/25] fix spelling Signed-off-by: mohammad alqudah --- .../autoware/behavior_path_lane_change_module/utils/utils.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 c57b5b8a37e66..2ec9d4cdf4e4e 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 @@ -144,10 +144,10 @@ bool isParkedObject( * * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, * and transient data. - * @param lane_change_path Cadidate lane change path to apply checks on. + * @param lane_change_path Candidate lane change path to apply checks on. * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include * target lane leading static objects). - * @param object_debug Collision check debug struct to be updated if any of the traget objects + * @param object_debug Collision check debug struct to be updated if any of the target objects * satisfy the conditions. * @return bool True if conditions to delay lane change are met */ From ca5367daff9d8c76834acb83c8de877112b5596a Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 27 Nov 2024 11:14:04 +0900 Subject: [PATCH 19/25] add delay parameters to README Signed-off-by: mohammad alqudah --- .../README.md | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 6ebe79c180e95..2458fa85c61b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -828,8 +828,6 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | | `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | | `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | -| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | | `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | @@ -860,6 +858,15 @@ The following parameters are used to judge lane change completion. | `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | | `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | +### Delay Lane Change + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------ | ---- | ------ | ----------------------------------------------------------------------------------------------------- | ------------- | +| `delay_lane_change.enable` | [-] | bool | Flag to enable/disable lane change delay feature | true | +| `delay_lane_change.check_only_parked_vehicle` | [-] | bool | Flag to limit delay feature for only parked vehicles | false | +| `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | +| `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | + ### Collision checks #### Target Objects From ace35b000c8476e5f7a290835ae354d1b8e0d704 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 27 Nov 2024 14:09:09 +0900 Subject: [PATCH 20/25] add documentation for delay lane change behavior Signed-off-by: mohammad alqudah --- .../README.md | 65 ++ .../images/delay_lane_change_1.drawio.svg | 657 +++++++++++++++++ .../images/delay_lane_change_2.drawio.svg | 625 ++++++++++++++++ .../images/delay_lane_change_3.drawio.svg | 683 ++++++++++++++++++ .../images/delay_lane_change_4.drawio.svg | 683 ++++++++++++++++++ .../images/delay_lane_change_5.drawio.svg | 677 +++++++++++++++++ 6 files changed, 3390 insertions(+) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 2458fa85c61b4..d24a89bf03000 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -351,6 +351,71 @@ stop @enduml ``` +#### Delay Lane Change Check + +In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle. +To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected. + +1. The distance from object to terminal end is sufficient to perform lane change +2. The distance to object is less than the lane changing length +3. The distance from object to next object is sufficient to perform lane change + +If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked. + +The following flow chart illustrates the delay lane change check. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #White + +start +if (Is target objects, candidate path, OR current lane path empty?) then (yes) + #LightPink:Return false; + stop +else (no) +endif + +:Start checking objects from closest to furthest; +repeat + if (Is distance from object to terminal sufficient) then (yes) + else (no) + #LightPink:Return false; + stop + endif + + if (Is distance to object less than lane changing length) then (yes) + else (no) + if (Is only check parked vehicles and vehicle is not parked) then (yes) + else (no) + if(Is last object OR distance to next object is sufficient) then (yes) + #LightGreen: Return true; + stop + else (no) + endif + endif + endif + repeat while (Is finished checking all objects) is (FALSE) + +#LightPink: Return false; +stop + +@enduml +``` + +The following figures demonstrate different situations under which will or will not be triggered: + +1. Delay lane change will be triggered as there is sufficient distance ahead. + ![delay lane change 1](./images/delay_lane_change_1.drawio.svg) +2. Delay lane change will NOT be triggered as there is no sufficient distance ahead + ![delay lane change 2](./images/delay_lane_change_2.drawio.svg) +3. Delay lane change will be triggered by fist NPC as there is sufficient distance ahead. + ![delay lane change 3](./images/delay_lane_change_3.drawio.svg) +4. Delay lane change will be triggered by second NPC as there is sufficient distance ahead + ![delay lane change 4](./images/delay_lane_change_4.drawio.svg) +5. Delay lane change will NOT be triggered as there is no sufficient distance ahead. + ![delay lane change 5](./images/delay_lane_change_5.drawio.svg) + #### Candidate Path's Safety check See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg new file mode 100644 index 0000000000000..f5f3e4e88559b --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg @@ -0,0 +1,657 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg new file mode 100644 index 0000000000000..8237ac312aadb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg @@ -0,0 +1,625 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg new file mode 100644 index 0000000000000..2bcbfb5cdb93d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg new file mode 100644 index 0000000000000..b38092183db14 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg new file mode 100644 index 0000000000000..d4abb53a84999 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg @@ -0,0 +1,677 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From bf669d232105312461b2041e1590e3e981d2689f Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 27 Nov 2024 15:06:11 +0900 Subject: [PATCH 21/25] Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../src/utils/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 05334cb2aea1e..91cff1d1bb494 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 @@ -763,8 +763,8 @@ bool is_delay_lane_change( const auto min_lon_acc = common_data_ptr->lc_param_ptr->trajectory.min_longitudinal_acc; const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc)); auto is_sufficient_gap = [&gap_threshold]( - const ExtendedPredictedObject & current_obj, - const ExtendedPredictedObject & next_obj) { + const auto & current_obj, + const auto & next_obj) { const auto curr_obj_half_length = current_obj.shape.dimensions.x; const auto next_obj_half_length = next_obj.shape.dimensions.x; const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; From c7c20936decf84a7d7fbf022f48fcc82c24b3a7c Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 27 Nov 2024 15:06:43 +0900 Subject: [PATCH 22/25] Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../src/utils/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 91cff1d1bb494..0b212949ee8ce 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 @@ -741,7 +741,7 @@ bool isParkedObject( bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & target_objects, + const ExtendedPredictedObjects & target_objects, CollisionCheckDebugMap & object_debug) { const auto & current_lane_path = common_data_ptr->current_lanes_path; From 1b476b6951a49e8225bafab0de91a0aaefbe6335 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 27 Nov 2024 16:49:01 +0900 Subject: [PATCH 23/25] Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../src/utils/utils.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 0b212949ee8ce..a369328007db5 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 @@ -774,8 +774,7 @@ bool is_delay_lane_change( const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; - auto it = target_objects.begin(); - for (; it < target_objects.end(); ++it) { + for (auto it = target_objects.begin(); it < target_objects.end(); ++it) { if (is_near_end(*it)) break; if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue; From 2e25b2cb6cf75d711b0d3c489861596f57b53d12 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 27 Nov 2024 16:54:10 +0900 Subject: [PATCH 24/25] run pre-commit checks Signed-off-by: mohammad alqudah --- .../src/utils/utils.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) 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 a369328007db5..ad0cf87c6172b 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 @@ -741,8 +741,7 @@ bool isParkedObject( bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const ExtendedPredictedObjects & target_objects, - CollisionCheckDebugMap & object_debug) + const ExtendedPredictedObjects & target_objects, CollisionCheckDebugMap & object_debug) { const auto & current_lane_path = common_data_ptr->current_lanes_path; @@ -762,9 +761,7 @@ bool is_delay_lane_change( const auto ego_vel = common_data_ptr->get_ego_speed(); const auto min_lon_acc = common_data_ptr->lc_param_ptr->trajectory.min_longitudinal_acc; const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc)); - auto is_sufficient_gap = [&gap_threshold]( - const auto & current_obj, - const auto & next_obj) { + auto is_sufficient_gap = [&gap_threshold](const auto & current_obj, const auto & next_obj) { const auto curr_obj_half_length = current_obj.shape.dimensions.x; const auto next_obj_half_length = next_obj.shape.dimensions.x; const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; From 1f7f61513acadbc8c82f0d3536a4973f972bc1d1 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Thu, 28 Nov 2024 10:43:50 +0900 Subject: [PATCH 25/25] only check for delay lc if feature is enabled Signed-off-by: mohammad alqudah --- .../src/utils/utils.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 ad0cf87c6172b..db0c1cb37abbb 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 @@ -744,9 +744,10 @@ bool is_delay_lane_change( const ExtendedPredictedObjects & target_objects, CollisionCheckDebugMap & object_debug) { const auto & current_lane_path = common_data_ptr->current_lanes_path; + const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; if ( - target_objects.empty() || lane_change_path.path.points.empty() || + !delay_lc_param.enable || target_objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { return false; } @@ -769,8 +770,6 @@ bool is_delay_lane_change( return gap_length > gap_threshold; }; - const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; - for (auto it = target_objects.begin(); it < target_objects.end(); ++it) { if (is_near_end(*it)) break;