From c171941c938c059d72cae90a17fbca1b1028d0dc Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 3 Oct 2023 11:19:09 +0900 Subject: [PATCH] more refactor Signed-off-by: Mamoru Sobue --- .../package.xml | 4 -- .../src/scene_intersection.cpp | 48 ++++++++++++------- .../src/scene_intersection.hpp | 2 - .../src/util.cpp | 2 +- .../src/util.hpp | 8 ---- 5 files changed, 31 insertions(+), 33 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 2fd733f167546..0c9b3407d0f38 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -23,11 +23,9 @@ autoware_perception_msgs behavior_velocity_planner_common geometry_msgs - grid_map_core interpolation lanelet2_extension libopencv-dev - magic_enum motion_utils nav_msgs pluginlib @@ -41,8 +39,6 @@ vehicle_info_util visualization_msgs - grid_map_rviz_plugin - ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 84f0ed6c421a8..0f47fecc69453 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,7 +17,6 @@ #include "util.hpp" #include -#include #include #include #include @@ -832,6 +831,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( "Path has no interval on intersection lane " + std::to_string(lane_id_)}; } + // cache intesersection lane information because it is invariant const auto & current_pose = planner_data_->current_odometry->pose; const auto lanelets_on_path = planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); @@ -844,12 +844,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } auto & intersection_lanelets = intersection_lanelets_.value(); + // at the very first time of registration of this module, the path may not be conflicting with the + // attention area, so update() is called to update the internal data as well as traffic light info const auto traffic_prioritized_level = util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; intersection_lanelets.update(is_prioritized, interpolated_path_info); + // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { @@ -857,7 +860,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto first_conflicting_area = first_conflicting_area_opt.value(); + // generate all stop line candidates + // see the doc for struct IntersectionStopLines const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); + /// even if the attention area is null, stuck vehicle stop line needs to be generated from + /// conflicting lanes const auto & dummy_first_attention_area = first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; const double peeking_offset = @@ -876,6 +883,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines; + // see the doc for struct PathLanelets + const auto & conflicting_area = intersection_lanelets.conflicting_area(); + const auto path_lanelets_opt = util::generatePathLanelets( + lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, + conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, + planner_data_->vehicle_info_.vehicle_width_m); + if (!path_lanelets_opt.has_value()) { + return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; + } + const auto path_lanelets = path_lanelets_opt.value(); + // utility functions auto fromEgoDist = [&](const size_t index) { return motion_utils::calcSignedArcLength(path->points, closest_idx, index); @@ -895,18 +913,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return state_machine.getState() == StateMachine::State::GO; }; - const auto & conflicting_area = intersection_lanelets.conflicting_area(); - const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, - conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); - if (!path_lanelets_opt.has_value()) { - return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; - } - const auto path_lanelets = path_lanelets_opt.value(); - + // stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected && stuck_stop_line_idx_opt) { auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); const bool stopped_at_stuck_line = stoppedForDuration( @@ -924,11 +933,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } + // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { return IntersectionModule::Indecisive{"attention area is empty"}; } const auto first_attention_area = first_attention_area_opt.value(); + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line if (!default_stop_line_idx_opt) { return IntersectionModule::Indecisive{"default stop line is null"}; } @@ -962,6 +974,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, eog has already passed the interection if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { return IntersectionModule::Indecisive{"occlusion stop line is null"}; } @@ -986,7 +1000,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - // calculate dynamic collision around detection area + // calculate dynamic collision around attention area const double time_to_restart = (is_go_out_ || is_prioritized) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - @@ -1043,11 +1057,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // check safety + // distinguish if ego detected occlusion or RTC detects occlusion const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); - const bool is_occlusion_state = - (occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || - ext_occlusion_requested); + const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); // Safe if (!is_occlusion_state && !has_collision_with_margin) { return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; @@ -1062,7 +1074,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, before_creep_state_machine_); if (stopped_at_default_line) { - // In this case ego will temporarily stop before entering attention area + // in this case ego will temporarily stop before entering attention area const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 2bf3df70e7a4a..7bd923ed6198c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -18,9 +18,7 @@ #include "util_type.hpp" #include -#include #include -#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 37dc83cbdf761..f381925b13208 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -16,10 +16,10 @@ #include "util_type.hpp" +#include #include #include #include -#include #include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 7ba894bd9d32a..6acc1d60443bf 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,14 +20,6 @@ #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include