Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(intersection): memorize last observed valid traffic light information #6048

Closed
wants to merge 1 commit into from
Closed

feat(intersection): memorize last observed valid traffic light information #6048

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
84 changes: 48 additions & 36 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 Tier IV, Inc.

Check notice on line 1 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File 

The lines of code increases from 2917 to 2924, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 9.62 to 9.47, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.

Check notice on line 1 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Primitive Obsession 

The ratio of primitive types in function arguments increases from 33.33% to 42.97%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -940,34 +940,51 @@
return true;
}

bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane)
bool IntersectionModule::isGreenSolidOn() const
{
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;

if (!last_tl_valid_observation_) {
// the info of this traffic light is not available
return false;
}
const auto & tl_info = last_tl_valid_observation_.value();
for (auto && tl_light : tl_info.signal.elements) {
if (
tl_light.color == TrafficSignalElement::GREEN &&
tl_light.shape == TrafficSignalElement::CIRCLE) {
return true;
}
}
return false;
}

void IntersectionModule::updateTrafficLightObservation(lanelet::ConstLanelet lane)
{
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;

std::optional<lanelet::Id> tl_id = std::nullopt;
for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
tl_id = tl_reg_elem->id();
break;
}
if (!tl_id) {
// this lane has no traffic light
return false;
return;
}
const auto tl_info_opt = planner_data_->getTrafficSignal(
tl_id.value(), true /* traffic light module keeps last observation*/);
if (!tl_info_opt) {
// the info of this traffic light is not available
return false;
// if the info of this traffic light is not available, keep last observation
return;
}
const auto & tl_info = tl_info_opt.value();
for (auto && tl_light : tl_info.signal.elements) {
if (
tl_light.color == TrafficSignalElement::GREEN &&
tl_light.shape == TrafficSignalElement::CIRCLE) {
return true;
for (const auto & tl_light : tl_info.signal.elements) {
if (tl_light.color == TrafficSignalElement::UNKNOWN) {
return;
}
}
return false;
last_tl_valid_observation_ = tl_info;

Check notice on line 987 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Method 

IntersectionModule::isGreenSolidOn is no longer above the threshold for cyclomatic complexity. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
}

IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
Expand Down Expand Up @@ -1002,271 +1019,273 @@
}
auto & intersection_lanelets = intersection_lanelets_.value();

updateTrafficLightObservation(assigned_lanelet);

// 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 = getTrafficPrioritizedLevel(assigned_lanelet);
const auto traffic_prioritized_level = getTrafficPrioritizedLevel();
const bool is_prioritized =
traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;
intersection_lanelets.update(
is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr);

// this is abnormal
const auto & conflicting_lanelets = intersection_lanelets.conflicting();
const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area();
const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane();
if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) {
return IntersectionModule::Indecisive{"conflicting area is empty"};
}
const auto & first_conflicting_lane = first_conflicting_lane_opt.value();
const auto & first_conflicting_area = first_conflicting_area_opt.value();
const auto & second_attention_area_opt = intersection_lanelets.second_attention_area();

// generate all stop line candidates
// see the doc for struct IntersectionStopLines
/// even if the attention area is null, stuck vehicle stop line needs to be generated from
/// conflicting lanes
const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane()
? intersection_lanelets.first_attention_lane().value()
: first_conflicting_lane;

const auto intersection_stoplines_opt = generateIntersectionStopLines(
assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt,
interpolated_path_info, path);
if (!intersection_stoplines_opt) {
return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"};
}
const auto & intersection_stoplines = intersection_stoplines_opt.value();
const auto closest_idx = intersection_stoplines.closest_idx;
const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline;
const auto default_stopline_idx_opt = intersection_stoplines.default_stopline;
const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline;
const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline;
const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line;
const auto occlusion_wo_tl_pass_judge_line_idx =
intersection_stoplines.occlusion_wo_tl_pass_judge_line;

// see the doc for struct PathLanelets
const auto & first_attention_area_opt = intersection_lanelets.first_attention_area();
const auto & conflicting_area = intersection_lanelets.conflicting_area();
const auto path_lanelets_opt = generatePathLanelets(
lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area,
first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx);
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);
};
auto stoppedForDuration =
[&](const size_t pos, const double duration, StateMachine & state_machine) {
const double dist_stopline = fromEgoDist(pos);
const bool approached_dist_stopline =
(std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin);
const bool over_stopline = (dist_stopline < 0.0);
const bool is_stopped_duration = planner_data_->isVehicleStopped(duration);
if (over_stopline) {
state_machine.setState(StateMachine::State::GO);
} else if (is_stopped_duration && approached_dist_stopline) {
state_machine.setState(StateMachine::State::GO);
}
return state_machine.getState() == StateMachine::State::GO;
};
auto stoppedAtPosition = [&](const size_t pos, const double duration) {
const double dist_stopline = fromEgoDist(pos);
const bool approached_dist_stopline =
(std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin);
const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin);
const bool is_stopped = planner_data_->isVehicleStopped(duration);
if (over_stopline) {
return true;
} else if (is_stopped && approached_dist_stopline) {
return true;
}
return false;
};

// 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 = checkStuckVehicleInIntersection(path_lanelets, &debug_data_);
const bool is_first_conflicting_lane_private =
(std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0);
if (stuck_detected) {
if (
is_first_conflicting_lane_private &&
planner_param_.stuck_vehicle.disable_against_private_lane) {
// do nothing
} else {
std::optional<size_t> stopline_idx = std::nullopt;
if (stuck_stopline_idx_opt) {
const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) <
-planner_param_.common.stopline_overshoot_margin;
if (!is_over_stuck_stopline) {
stopline_idx = stuck_stopline_idx_opt.value();
}
}
if (!stopline_idx) {
if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) {
stopline_idx = default_stopline_idx_opt.value();
} else if (
first_attention_stopline_idx_opt &&
fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) {
stopline_idx = closest_idx;
}
}
if (stopline_idx) {
return IntersectionModule::StuckStop{
closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt};
}
}
}

// 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_stopline_idx_opt) {
return IntersectionModule::Indecisive{"default stop line is null"};
}
// 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 intersection
if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) {
return IntersectionModule::Indecisive{"occlusion stop line is null"};
}
const auto default_stopline_idx = default_stopline_idx_opt.value();
const bool is_over_default_stopline =
util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx);
const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx;
const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value();
const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value();

const auto & adjacent_lanelets = intersection_lanelets.adjacent();
const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention();
const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area();
debug_data_.attention_area = intersection_lanelets.attention_area();
debug_data_.occlusion_attention_area = occlusion_attention_area;
debug_data_.adjacent_area = intersection_lanelets.adjacent_area();
debug_data_.first_attention_area = intersection_lanelets.first_attention_area();
debug_data_.second_attention_area = intersection_lanelets.second_attention_area();

// check occlusion on detection lane
if (!occlusion_attention_divisions_) {
occlusion_attention_divisions_ = generateDetectionLaneDivisions(
occlusion_attention_lanelets, routing_graph_ptr,
planner_data_->occupancy_grid->info.resolution);
}
const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value();

// get intersection area
const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
// filter objects
auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area);

const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection(
target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding(),
&debug_data_);
if (yield_stuck_detected) {
std::optional<size_t> stopline_idx = std::nullopt;
const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0;
const bool is_before_first_attention_stopline =
fromEgoDist(first_attention_stopline_idx) >= 0.0;
if (stuck_stopline_idx_opt) {
const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) <
-planner_param_.common.stopline_overshoot_margin;
if (!is_over_stuck_stopline) {
stopline_idx = stuck_stopline_idx_opt.value();
}
}
if (!stopline_idx) {
if (is_before_default_stopline) {
stopline_idx = default_stopline_idx;
} else if (is_before_first_attention_stopline) {
stopline_idx = closest_idx;
}
}
if (stopline_idx) {
return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()};
}
}

const bool is_amber_or_red =
(traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) ||
(traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED);
auto occlusion_status =
(enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red)
? getOcclusionStatus(
occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info,
occlusion_attention_divisions, target_objects)
: OcclusionType::NOT_OCCLUDED;
occlusion_stop_state_machine_.setStateWithMarginTime(
occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP,
logger_.get_child("occlusion_stop"), *clock_);
const bool is_occlusion_cleared_with_margin =
(occlusion_stop_state_machine_.getState() == StateMachine::State::GO);
// distinguish if ego detected occlusion or RTC detects occlusion
const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_);
if (ext_occlusion_requested) {
occlusion_status = OcclusionType::RTC_OCCLUDED;
}
const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested);
if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) {
occlusion_status = prev_occlusion_status_;
} else {
prev_occlusion_status_ = occlusion_status;
}

// TODO(Mamoru Sobue): this part needs more formal handling
const size_t pass_judge_line_idx = [=]() {
if (enable_occlusion_detection_) {
// if occlusion detection is enabled, pass_judge position is beyond the boundary of first
// attention area
if (has_traffic_light_) {
return occlusion_stopline_idx;
} else if (is_occlusion_state) {
// if there is no traffic light and occlusion is detected, pass_judge position is beyond
// the boundary of first attention area
return occlusion_wo_tl_pass_judge_line_idx;
} else {
// if there is no traffic light and occlusion is not detected, pass_judge position is
// default
return default_pass_judge_line_idx;
}
}
return default_pass_judge_line_idx;
}();
debug_data_.pass_judge_wall_pose =
planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path);
const bool is_over_pass_judge_line =
util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx);
const double vel_norm = std::hypot(
planner_data_->current_velocity->twist.linear.x,
planner_data_->current_velocity->twist.linear.y);
const bool keep_detection =
(vel_norm < planner_param_.collision_detection.keep_detection_velocity_threshold);
const bool was_safe = std::holds_alternative<IntersectionModule::Safe>(prev_decision_result_);
// if ego is over the pass judge line and not stopped
if (is_over_default_stopline && !is_over_pass_judge_line && keep_detection) {
RCLCPP_DEBUG(logger_, "is_over_default_stopline && !is_over_pass_judge_line && keep_detection");
// do nothing
} else if (
(was_safe && is_over_default_stopline && is_over_pass_judge_line && is_go_out_) ||
is_permanent_go_) {
// is_go_out_: previous RTC approval
// activated_: current RTC approval
is_permanent_go_ = true;
return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"};
}

// If there are any vehicles on the attention area when ego entered the intersection on green
// light, do pseudo collision detection because the vehicles are very slow and no collisions may
// be detected. check if ego vehicle entered assigned lanelet
const bool is_green_solid_on = isGreenSolidOn(assigned_lanelet);
const bool is_green_solid_on = isGreenSolidOn();

Check notice on line 1288 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Complex Method 

IntersectionModule::modifyPathVelocityDetail already has high cyclomatic complexity, and now it increases in Lines of Code from 378 to 379. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
if (is_green_solid_on) {
if (!initial_green_light_observed_time_) {
const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();
Expand Down Expand Up @@ -1419,25 +1438,15 @@
}
}

TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane)
TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel() const
{
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;

std::optional<lanelet::Id> tl_id = std::nullopt;
for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
tl_id = tl_reg_elem->id();
break;
}
if (!tl_id) {
// this lane has no traffic light
if (!last_tl_valid_observation_) {
// if no information is available from the beginning, NOT_PRIORITIZED
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
}
const auto tl_info_opt = planner_data_->getTrafficSignal(
tl_id.value(), true /* traffic light module keeps last observation*/);
if (!tl_info_opt) {
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
}
const auto & tl_info = tl_info_opt.value();
const auto & tl_info = last_tl_valid_observation_.value();

Check notice on line 1449 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Method 

IntersectionModule::getTrafficPrioritizedLevel is no longer above the threshold for cyclomatic complexity. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
bool has_amber_signal{false};
for (auto && tl_light : tl_info.signal.elements) {
if (tl_light.color == TrafficSignalElement::AMBER) {
Expand Down Expand Up @@ -1466,7 +1475,8 @@

IntersectionLanelets IntersectionModule::getObjectiveLanelets(
lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path)
const lanelet::ConstLanelet assigned_lanelet,
const lanelet::ConstLanelets & lanelets_on_path) const

Check notice on line 1479 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Complex Method 

IntersectionModule::getObjectiveLanelets already has high cyclomatic complexity, and now it increases in Lines of Code from 162 to 163. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
{
const double detection_area_length = planner_param_.common.attention_area_length;
const double occlusion_detection_area_length =
Expand Down Expand Up @@ -1669,7 +1679,7 @@
const lanelet::ConstLanelet & first_attention_lane,
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
const util::InterpolatedPathInfo & interpolated_path_info,
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path)
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const

Check notice on line 1682 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

IntersectionModule::generateIntersectionStopLines is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
{
const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline;
const double stopline_margin = planner_param_.common.default_stopline_margin;
Expand Down Expand Up @@ -1897,7 +1907,8 @@
* @return true when the stop point is defined on map.
*/
std::optional<size_t> IntersectionModule::getStopLineIndexFromMap(
const util::InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet)
const util::InterpolatedPathInfo & interpolated_path_info,
lanelet::ConstLanelet assigned_lanelet) const
{
const auto & path = interpolated_path_info.path;
const auto & lane_interval = interpolated_path_info.lane_id_interval.value();
Expand Down Expand Up @@ -2043,7 +2054,7 @@
const lanelet::CompoundPolygon3d & first_conflicting_area,
const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,
const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx)
const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx) const

Check notice on line 2057 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

IntersectionModule::generatePathLanelets is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
{
const double width = planner_data_->vehicle_info_.vehicle_width_m;
static constexpr double path_lanelet_interval = 1.5;
Expand Down Expand Up @@ -2110,7 +2121,7 @@
}

bool IntersectionModule::checkStuckVehicleInIntersection(
const PathLanelets & path_lanelets, DebugData * debug_data)
const PathLanelets & path_lanelets, DebugData * debug_data) const
{
using lanelet::utils::getArcCoordinates;
using lanelet::utils::getLaneletLength3d;
Expand Down Expand Up @@ -2265,7 +2276,7 @@

bool IntersectionModule::checkYieldStuckVehicleInIntersection(
const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info,
const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data)
const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) const
{
const bool yield_stuck_detection_direction = [&]() {
return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) ||
Expand Down Expand Up @@ -2415,7 +2426,7 @@
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects,
const PathLanelets & path_lanelets, const size_t closest_idx,
const size_t last_intersection_stopline_candidate_idx, const double time_delay,
const TrafficPrioritizedLevel & traffic_prioritized_level)
const TrafficPrioritizedLevel & traffic_prioritized_level) const

Check notice on line 2429 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

IntersectionModule::checkCollision is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check notice on line 2429 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Brain Method 

IntersectionModule::checkCollision is no longer a brain method
{
using lanelet::utils::getArcCoordinates;
using lanelet::utils::getPolygonFromArcLength;
Expand Down Expand Up @@ -2699,7 +2710,7 @@
}

void IntersectionModule::cutPredictPathWithDuration(
TargetObjects * target_objects, const double time_thr)
TargetObjects * target_objects, const double time_thr) const
{
const rclcpp::Time current_time = clock_->now();
for (auto & target_object : target_objects->all_attention_objects) { // each objects
Expand All @@ -2724,7 +2735,7 @@
TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx,
const size_t last_intersection_stopline_candidate_idx, const double time_delay,
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array)
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const

Check notice on line 2738 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

IntersectionModule::calcIntersectionPassingTime is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
{
const double intersection_velocity =
planner_param_.collision_detection.velocity_profile.default_velocity;
Expand Down Expand Up @@ -2936,7 +2947,7 @@
const lanelet::CompoundPolygon3d & first_attention_area,
const util::InterpolatedPathInfo & interpolated_path_info,
const std::vector<lanelet::ConstLineString3d> & lane_divisions,
const TargetObjects & target_objects)
const TargetObjects & target_objects) const

Check notice on line 2950 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

IntersectionModule::getOcclusionStatus is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check notice on line 2950 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Brain Method 

IntersectionModule::getOcclusionStatus is no longer a brain method
{
const auto & occ_grid = *planner_data_->occupancy_grid;
const auto & current_pose = planner_data_->current_odometry->pose;
Expand Down Expand Up @@ -3336,6 +3347,7 @@
const auto attention_non_preceding_ex_first_area =
getPolygon3dFromLanelets(attention_non_preceding_ex_first);
const auto second = getFirstPointInsidePolygonsByFootprint(

attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length);
if (second) {
second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second);
Expand Down
38 changes: 25 additions & 13 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -616,6 +616,9 @@ class IntersectionModule : public SceneModuleInterface

std::optional<IntersectionLanelets> intersection_lanelets_{std::nullopt};

// memorize last observed non-UNKNOWN traffic light information
std::optional<TrafficSignalStamped> last_tl_valid_observation_{std::nullopt};

// for pass judge decision
bool is_go_out_{false};
bool is_permanent_go_{false};
Expand Down Expand Up @@ -662,7 +665,7 @@ class IntersectionModule : public SceneModuleInterface
* @fn
* @brief find TrafficPrioritizedLevel
*/
TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane);
TrafficPrioritizedLevel getTrafficPrioritizedLevel() const;

/**
* @fn
Expand All @@ -671,7 +674,8 @@ class IntersectionModule : public SceneModuleInterface
IntersectionLanelets getObjectiveLanelets(
lanelet::LaneletMapConstPtr lanelet_map_ptr,
lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path);
const lanelet::ConstLanelet assigned_lanelet,
const lanelet::ConstLanelets & lanelets_on_path) const;

/**
* @fn
Expand All @@ -683,15 +687,15 @@ class IntersectionModule : public SceneModuleInterface
const lanelet::ConstLanelet & first_attention_lane,
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
const util::InterpolatedPathInfo & interpolated_path_info,
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const;

/**
* @fn
* @brief find the associated stopline road marking of assigned lanelet
*/
std::optional<size_t> getStopLineIndexFromMap(
const util::InterpolatedPathInfo & interpolated_path_info,
lanelet::ConstLanelet assigned_lanelet);
lanelet::ConstLanelet assigned_lanelet) const;

/**
* @fn
Expand All @@ -703,21 +707,23 @@ class IntersectionModule : public SceneModuleInterface
const lanelet::CompoundPolygon3d & first_conflicting_area,
const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,
const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx);
const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
const size_t closest_idx) const;

/**
* @fn
* @brief check stuck
*/
bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets, DebugData * debug_data);
bool checkStuckVehicleInIntersection(
const PathLanelets & path_lanelets, DebugData * debug_data) const;

/**
* @fn
* @brief check yield stuck
*/
bool checkYieldStuckVehicleInIntersection(
const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info,
const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data);
const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) const;

/**
* @fn
Expand All @@ -735,13 +741,13 @@ class IntersectionModule : public SceneModuleInterface
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects,
const PathLanelets & path_lanelets, const size_t closest_idx,
const size_t last_intersection_stopline_candidate_idx, const double time_delay,
const TrafficPrioritizedLevel & traffic_prioritized_level);
const TrafficPrioritizedLevel & traffic_prioritized_level) const;

std::optional<size_t> checkAngleForTargetLanelets(
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
const bool is_parked_vehicle) const;

void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr);
void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr) const;

/**
* @fn
Expand All @@ -751,7 +757,7 @@ class IntersectionModule : public SceneModuleInterface
TimeDistanceArray calcIntersectionPassingTime(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx,
const size_t last_intersection_stopline_candidate_idx, const double time_delay,
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array);
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const;

std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
lanelet::ConstLanelets detection_lanelets,
Expand All @@ -767,13 +773,13 @@ class IntersectionModule : public SceneModuleInterface
const lanelet::CompoundPolygon3d & first_attention_area,
const util::InterpolatedPathInfo & interpolated_path_info,
const std::vector<lanelet::ConstLineString3d> & lane_divisions,
const TargetObjects & target_objects);
const TargetObjects & target_objects) const;

/*
* @fn
* @brief check if associated traffic light is green
*/
bool isGreenSolidOn(lanelet::ConstLanelet lane);
bool isGreenSolidOn() const;

/*
bool IntersectionModule::checkFrontVehicleDeceleration(
Expand All @@ -783,7 +789,13 @@ class IntersectionModule : public SceneModuleInterface
const double assumed_front_car_decel);
*/

DebugData debug_data_;
/*
* @fn
* @brief update last_tl_valid_observation_
*/
void updateTrafficLightObservation(lanelet::ConstLanelet lane);

mutable DebugData debug_data_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr decision_state_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr ego_ttc_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr object_ttc_pub_;
Expand Down
Loading