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.

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_occlusion): quickly delete occlusion wall on change from green to yellow/red #6608

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -148,17 +148,18 @@
intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
{
const auto traffic_prioritized_level = getTrafficPrioritizedLevel();
const bool is_prioritized =
traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;

const auto prepare_data = prepareIntersectionData(is_prioritized, path);
const auto prepare_data = prepareIntersectionData(path);

Check warning on line 151 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp#L151

Added line #L151 was not covered by tests
if (!prepare_data) {
return prepare_data.err();
}
const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok();
const auto & intersection_lanelets = intersection_lanelets_.value();

// NOTE: this level is based on the updateTrafficSignalObservation() which is latest
const auto traffic_prioritized_level = getTrafficPrioritizedLevel();
const bool is_prioritized =

Check warning on line 160 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp#L160

Added line #L160 was not covered by tests
traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;

// ==========================================================================================
// stuck detection
//
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -442,6 +442,9 @@ class IntersectionModule : public SceneModuleInterface
//! unavailable)
std::optional<std::pair<lanelet::Id, lanelet::ConstPoint3d>> tl_id_and_point_;
std::optional<TrafficSignalStamped> last_tl_valid_observation_{std::nullopt};

//! save previous priority level to detect change from NotPrioritized to Prioritized
TrafficPrioritizedLevel previous_prioritized_level_{TrafficPrioritizedLevel::NOT_PRIORITIZED};
/** @} */

private:
Expand Down Expand Up @@ -548,7 +551,7 @@ class IntersectionModule : public SceneModuleInterface
* To simplify modifyPathVelocityDetail(), this function is used at first
*/
intersection::Result<BasicData, intersection::InternalError> prepareIntersectionData(
const bool is_prioritized, PathWithLaneId * path);
PathWithLaneId * path);

/**
* @brief find the associated stopline road marking of assigned lanelet
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -63,10 +63,23 @@
!is_amber_or_red_or_no_tl_info_ever)
? detectOcclusion(interpolated_path_info)
: NotOccluded{};
occlusion_stop_state_machine_.setStateWithMarginTime(
std::holds_alternative<NotOccluded>(occlusion_status) ? StateMachine::State::GO
: StateMachine::STOP,
logger_.get_child("occlusion_stop"), *clock_);

// ==========================================================================================
// if the traffic light changed from green to yellow/red, hysteresis time for occlusion is
// unnecessary
// ==========================================================================================
const auto transition_to_prioritized =
(previous_prioritized_level_ == TrafficPrioritizedLevel::NOT_PRIORITIZED &&
traffic_prioritized_level != TrafficPrioritizedLevel::NOT_PRIORITIZED);
if (transition_to_prioritized) {
occlusion_stop_state_machine_.setState(StateMachine::State::GO);
} else {
occlusion_stop_state_machine_.setStateWithMarginTime(
std::holds_alternative<NotOccluded>(occlusion_status) ? StateMachine::State::GO
: StateMachine::STOP,
logger_.get_child("occlusion_stop"), *clock_);
}

Check warning on line 82 in planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

IntersectionModule::getOcclusionStatus increases in cyclomatic complexity from 13 to 15, threshold = 9. 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 bool is_occlusion_cleared_with_margin =
(occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection
// distinguish if ego detected occlusion or RTC detects occlusion
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,7 @@
using intersection::Result;

Result<IntersectionModule::BasicData, intersection::InternalError>
IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path)
IntersectionModule::prepareIntersectionData(PathWithLaneId * path)

Check warning on line 169 in planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp#L169

Added line #L169 was not covered by tests
{
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
Expand All @@ -175,6 +175,18 @@
const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
const auto & current_pose = planner_data_->current_odometry->pose;

// ==========================================================================================
// update traffic light information
// updateTrafficSignalObservation() must be called at first because other traffic signal
// fuctions use last_valid_observation_
// ==========================================================================================
// save previous information before calling updateTrafficSignalObservation()
previous_prioritized_level_ = getTrafficPrioritizedLevel();
updateTrafficSignalObservation();
const auto traffic_prioritized_level = getTrafficPrioritizedLevel();
const bool is_prioritized =

Check warning on line 187 in planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp#L187

Added line #L187 was not covered by tests
traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;

// spline interpolation
const auto interpolated_path_info_opt = util::generateInterpolatedPath(
lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_);
Expand Down Expand Up @@ -264,13 +276,7 @@
planner_data_->occupancy_grid->info.resolution);
}

// ==========================================================================================
// update traffic light information
// updateTrafficSignalObservation() must be called at first to because other traffic signal
// fuctions use last_valid_observation_
// ==========================================================================================
if (has_traffic_light_) {

Check warning on line 279 in planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

IntersectionModule::prepareIntersectionData already has high cyclomatic complexity, and now it increases in Lines of Code from 97 to 101. 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.
updateTrafficSignalObservation();
const bool is_green_solid_on = isGreenSolidOn();
if (is_green_solid_on && !initial_green_light_observed_time_) {
const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();
Expand Down
Loading