Skip to content

Commit

Permalink
feat(intersection): memorize last observed valid traffic light inform…
Browse files Browse the repository at this point in the history 
…ation

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
  • Loading branch information
@soblin
soblin committed Feb 2, 2024
1 parent a3b92d1 commit 4257f3f
Show file tree
Hide file tree
Showing 3 changed files with 114 additions and 52 deletions.
108 changes: 69 additions & 39 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp> // for toGeomPoly
#include <behavior_velocity_planner_common/utilization/util.hpp>
#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <motion_utils/trajectory/trajectory.hpp>
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp> // for toPolygon2d
Expand Down Expand Up @@ -1143,25 +1144,11 @@ void IntersectionModule::reactRTCApproval(
bool IntersectionModule::isGreenSolidOn() const
{
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_);

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_) {
return false;
}
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;
}
const auto & tl_info = tl_info_opt.value();
const auto & tl_info = last_tl_valid_observation_.value();

Check notice on line 1151 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.
for (auto && tl_light : tl_info.signal.elements) {
if (
tl_light.color == TrafficSignalElement::GREEN &&
Expand All @@ -1176,38 +1163,81 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori
{
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;

auto corresponding_arrow = [&](const TrafficSignalElement & element) {
if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) {
return true;
}
if (turn_direction_ == "left" && element.shape == TrafficSignalElement::LEFT_ARROW) {
return true;
}
if (turn_direction_ == "right" && element.shape == TrafficSignalElement::RIGHT_ARROW) {
return true;
}
return false;
};
auto level = TrafficPrioritizedLevel::NOT_PRIORITIZED;
auto color = TrafficSignalElement::GREEN;
if (last_tl_valid_observation_) {
const auto & tl_info = last_tl_valid_observation_.value();
bool has_amber_signal{false};
for (auto && tl_light : tl_info.signal.elements) {
if (
tl_light.color == TrafficSignalElement::AMBER &&
tl_light.shape == TrafficSignalElement::CIRCLE) {
has_amber_signal = true;
}
if (
(tl_light.color == TrafficSignalElement::RED &&
tl_light.shape == TrafficSignalElement::CIRCLE) ||
(tl_light.color == TrafficSignalElement::GREEN && corresponding_arrow(tl_light))) {

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Conditional 

IntersectionModule::getTrafficPrioritizedLevel has 1 complex conditionals with 3 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
// NOTE: Return here since the red signal has the highest priority.
level = TrafficPrioritizedLevel::FULLY_PRIORITIZED;
color = TrafficSignalElement::RED;
break;
}
}
if (has_amber_signal) {
level = TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
color = TrafficSignalElement::AMBER;
}
}
if (tl_id_and_point_) {
const auto [tl_id, point] = tl_id_and_point_.value();
debug_data_.traffic_light_observation =
std::make_tuple(planner_data_->current_odometry->pose, point, color);
}
return level;
}

void IntersectionModule::updateTrafficSignalObservation()
{
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_);

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_and_point_) {
for (auto && tl_reg_elem :
lane.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>()) {
for (const auto & ls : tl_reg_elem->lightBulbs()) {
if (ls.hasAttribute("traffic_light_id")) {
tl_id_and_point_ = std::make_pair(tl_reg_elem->id(), ls.front());
break;
}
}
}
}
if (!tl_id) {
if (!tl_id_and_point_) {
// this lane has no traffic light
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
return;
}
const auto tl_info_opt = planner_data_->getTrafficSignal(
tl_id.value(), true /* traffic light module keeps last observation*/);
const auto [tl_id, point] = tl_id_and_point_.value();
const auto tl_info_opt =
planner_data_->getTrafficSignal(tl_id, true /* traffic light module keeps last observation*/);
if (!tl_info_opt) {
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
// the info of this traffic light is not available
return;
}
const auto & tl_info = tl_info_opt.value();
bool has_amber_signal{false};
for (auto && tl_light : tl_info.signal.elements) {
if (tl_light.color == TrafficSignalElement::AMBER) {
has_amber_signal = true;
}
if (tl_light.color == TrafficSignalElement::RED) {
// NOTE: Return here since the red signal has the highest priority.
return TrafficPrioritizedLevel::FULLY_PRIORITIZED;
}
}
if (has_amber_signal) {
return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
}
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
last_tl_valid_observation_ = tl_info;

Check notice on line 1240 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::getTrafficPrioritizedLevel increases in cyclomatic complexity from 10 to 18, 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.

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Deep, Nested Complexity 

IntersectionModule::updateTrafficSignalObservation has a nested complexity depth of 4, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
}

IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus(
Expand Down
24 changes: 24 additions & 0 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -209,6 +209,9 @@ class IntersectionModule : public SceneModuleInterface
std::optional<std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
nearest_occlusion_projection{std::nullopt};
std::optional<double> static_occlusion_with_traffic_light_timeout{std::nullopt};

std::optional<std::tuple<geometry_msgs::msg::Pose, lanelet::ConstPoint3d, uint8_t>>
traffic_light_observation{std::nullopt};
};

using TimeDistanceArray = std::vector<std::pair<double /* time*/, double /* distance*/>>;
Expand Down Expand Up @@ -388,6 +391,20 @@ class IntersectionModule : public SceneModuleInterface
intersection::ObjectInfoManager object_info_manager_;
/** @} */

private:
/**
***********************************************************
***********************************************************
***********************************************************
* @defgroup collision-variables [var] collision detection
* @{
*/
//! save the last UNKNOWN traffic light information of this intersection(keep even if the info got
//! unavailable)
std::optional<std::pair<lanelet::Id, lanelet::ConstPoint3d>> tl_id_and_point_;
std::optional<TrafficSignalStamped> last_tl_valid_observation_{std::nullopt};
/** @} */

private:
/**
***********************************************************
Expand Down Expand Up @@ -556,6 +573,13 @@ class IntersectionModule : public SceneModuleInterface
* @brief find TrafficPrioritizedLevel
*/
TrafficPrioritizedLevel getTrafficPrioritizedLevel() const;

/**
* @brief update the valid traffic signal information if still available, otherwise keep last
* observation
*/
void updateTrafficSignalObservation();

/** @} */

private:
Expand Down
34 changes: 21 additions & 13 deletions planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,19 +258,27 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL
planner_data_->occupancy_grid->info.resolution);
}

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();
const bool approached_assigned_lane =
motion_utils::calcSignedArcLength(
path->points, closest_idx,
tier4_autoware_utils::createPoint(
assigned_lane_begin_point.x(), assigned_lane_begin_point.y(),
assigned_lane_begin_point.z())) <
planner_param_.collision_detection.yield_on_green_traffic_light
.distance_to_assigned_lanelet_start;
if (approached_assigned_lane) {
initial_green_light_observed_time_ = clock_->now();
// ==========================================================================================
// update traffic light information
// updateTrafficSignalObservation() must be called at first to because other traffic signal
// fuctions use last_valid_observation_
// ==========================================================================================
if (has_traffic_light_) {
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();
const bool approached_assigned_lane =
motion_utils::calcSignedArcLength(
path->points, closest_idx,
tier4_autoware_utils::createPoint(
assigned_lane_begin_point.x(), assigned_lane_begin_point.y(),
assigned_lane_begin_point.z())) <
planner_param_.collision_detection.yield_on_green_traffic_light
.distance_to_assigned_lanelet_start;
if (approached_assigned_lane) {
initial_green_light_observed_time_ = clock_->now();
}

Check warning on line 281 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 increases in cyclomatic complexity from 14 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.
}
}

Expand Down

0 comments on commit 4257f3f

Please sign in to comment.