Skip to content

Commit 8daf8d3

Browse files
committed
feat(intersection): memorize last observed valid traffic light information
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent a3b92d1 commit 8daf8d3

File tree

3 files changed

+99
-39
lines changed

3 files changed

+99
-39
lines changed

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+69-39
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818

1919
#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp> // for toGeomPoly
2020
#include <behavior_velocity_planner_common/utilization/util.hpp>
21+
#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
2122
#include <lanelet2_extension/utility/utilities.hpp>
2223
#include <motion_utils/trajectory/trajectory.hpp>
2324
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp> // for toPolygon2d
@@ -1143,25 +1144,11 @@ void IntersectionModule::reactRTCApproval(
11431144
bool IntersectionModule::isGreenSolidOn() const
11441145
{
11451146
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
1146-
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
1147-
const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_);
11481147

1149-
std::optional<lanelet::Id> tl_id = std::nullopt;
1150-
for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
1151-
tl_id = tl_reg_elem->id();
1152-
break;
1153-
}
1154-
if (!tl_id) {
1155-
// this lane has no traffic light
1148+
if (!last_tl_valid_observation_) {
11561149
return false;
11571150
}
1158-
const auto tl_info_opt = planner_data_->getTrafficSignal(
1159-
tl_id.value(), true /* traffic light module keeps last observation*/);
1160-
if (!tl_info_opt) {
1161-
// the info of this traffic light is not available
1162-
return false;
1163-
}
1164-
const auto & tl_info = tl_info_opt.value();
1151+
const auto & tl_info = last_tl_valid_observation_.value();
11651152
for (auto && tl_light : tl_info.signal.elements) {
11661153
if (
11671154
tl_light.color == TrafficSignalElement::GREEN &&
@@ -1176,38 +1163,81 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori
11761163
{
11771164
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
11781165

1166+
auto corresponding_arrow = [&](const TrafficSignalElement & element) {
1167+
if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) {
1168+
return true;
1169+
}
1170+
if (turn_direction_ == "left" && element.shape == TrafficSignalElement::LEFT_ARROW) {
1171+
return true;
1172+
}
1173+
if (turn_direction_ == "right" && element.shape == TrafficSignalElement::RIGHT_ARROW) {
1174+
return true;
1175+
}
1176+
return false;
1177+
};
1178+
auto level = TrafficPrioritizedLevel::NOT_PRIORITIZED;
1179+
auto color = TrafficSignalElement::GREEN;
1180+
if (last_tl_valid_observation_) {
1181+
const auto & tl_info = last_tl_valid_observation_.value();
1182+
bool has_amber_signal{false};
1183+
for (auto && tl_light : tl_info.signal.elements) {
1184+
if (
1185+
tl_light.color == TrafficSignalElement::AMBER &&
1186+
tl_light.shape == TrafficSignalElement::CIRCLE) {
1187+
has_amber_signal = true;
1188+
}
1189+
if (
1190+
(tl_light.color == TrafficSignalElement::RED &&
1191+
tl_light.shape == TrafficSignalElement::CIRCLE) ||
1192+
(tl_light.color == TrafficSignalElement::GREEN && corresponding_arrow(tl_light))) {
1193+
// NOTE: Return here since the red signal has the highest priority.
1194+
level = TrafficPrioritizedLevel::FULLY_PRIORITIZED;
1195+
color = TrafficSignalElement::RED;
1196+
break;
1197+
}
1198+
}
1199+
if (has_amber_signal) {
1200+
level = TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
1201+
color = TrafficSignalElement::AMBER;
1202+
}
1203+
}
1204+
if (tl_id_and_point_) {
1205+
const auto [tl_id, point] = tl_id_and_point_.value();
1206+
debug_data_.traffic_light_observation =
1207+
std::make_tuple(planner_data_->current_odometry->pose, point, color);
1208+
}
1209+
return level;
1210+
}
1211+
1212+
void IntersectionModule::updateTrafficSignalObservation()
1213+
{
11791214
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
11801215
const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_);
11811216

1182-
std::optional<lanelet::Id> tl_id = std::nullopt;
1183-
for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
1184-
tl_id = tl_reg_elem->id();
1185-
break;
1217+
if (!tl_id_and_point_) {
1218+
for (auto && tl_reg_elem :
1219+
lane.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>()) {
1220+
for (const auto & ls : tl_reg_elem->lightBulbs()) {
1221+
if (ls.hasAttribute("traffic_light_id")) {
1222+
tl_id_and_point_ = std::make_pair(tl_reg_elem->id(), ls.front());
1223+
break;
1224+
}
1225+
}
1226+
}
11861227
}
1187-
if (!tl_id) {
1228+
if (!tl_id_and_point_) {
11881229
// this lane has no traffic light
1189-
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
1230+
return;
11901231
}
1191-
const auto tl_info_opt = planner_data_->getTrafficSignal(
1192-
tl_id.value(), true /* traffic light module keeps last observation*/);
1232+
const auto [tl_id, point] = tl_id_and_point_.value();
1233+
const auto tl_info_opt =
1234+
planner_data_->getTrafficSignal(tl_id, true /* traffic light module keeps last observation*/);
11931235
if (!tl_info_opt) {
1194-
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
1236+
// the info of this traffic light is not available
1237+
return;
11951238
}
11961239
const auto & tl_info = tl_info_opt.value();
1197-
bool has_amber_signal{false};
1198-
for (auto && tl_light : tl_info.signal.elements) {
1199-
if (tl_light.color == TrafficSignalElement::AMBER) {
1200-
has_amber_signal = true;
1201-
}
1202-
if (tl_light.color == TrafficSignalElement::RED) {
1203-
// NOTE: Return here since the red signal has the highest priority.
1204-
return TrafficPrioritizedLevel::FULLY_PRIORITIZED;
1205-
}
1206-
}
1207-
if (has_amber_signal) {
1208-
return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
1209-
}
1210-
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
1240+
last_tl_valid_observation_ = tl_info;
12111241
}
12121242

12131243
IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus(

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

+24
Original file line numberDiff line numberDiff line change
@@ -209,6 +209,9 @@ class IntersectionModule : public SceneModuleInterface
209209
std::optional<std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
210210
nearest_occlusion_projection{std::nullopt};
211211
std::optional<double> static_occlusion_with_traffic_light_timeout{std::nullopt};
212+
213+
std::optional<std::tuple<geometry_msgs::msg::Pose, lanelet::ConstPoint3d, uint8_t>>
214+
traffic_light_observation{std::nullopt};
212215
};
213216

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

394+
private:
395+
/**
396+
***********************************************************
397+
***********************************************************
398+
***********************************************************
399+
* @defgroup collision-variables [var] collision detection
400+
* @{
401+
*/
402+
//! save the last UNKNOWN traffic light information of this intersection(keep even if the info got
403+
//! unavailable)
404+
std::optional<std::pair<lanelet::Id, lanelet::ConstPoint3d>> tl_id_and_point_;
405+
std::optional<TrafficSignalStamped> last_tl_valid_observation_{std::nullopt};
406+
/** @} */
407+
391408
private:
392409
/**
393410
***********************************************************
@@ -556,6 +573,13 @@ class IntersectionModule : public SceneModuleInterface
556573
* @brief find TrafficPrioritizedLevel
557574
*/
558575
TrafficPrioritizedLevel getTrafficPrioritizedLevel() const;
576+
577+
/**
578+
* @brief update the valid traffic signal information if still available, otherwise keep last
579+
* observation
580+
*/
581+
void updateTrafficSignalObservation();
582+
559583
/** @} */
560584

561585
private:

planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -258,6 +258,12 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL
258258
planner_data_->occupancy_grid->info.resolution);
259259
}
260260

261+
// ==========================================================================================
262+
// update traffic light information
263+
// updateTrafficSignalObservation() must be called at first to because other traffic signal
264+
// fuctions use last_valid_observation_
265+
// ==========================================================================================
266+
updateTrafficSignalObservation();
261267
const bool is_green_solid_on = isGreenSolidOn();
262268
if (is_green_solid_on && !initial_green_light_observed_time_) {
263269
const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();

0 commit comments

Comments
 (0)