18
18
19
19
#include < behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp> // for toGeomPoly
20
20
#include < behavior_velocity_planner_common/utilization/util.hpp>
21
+ #include < lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
21
22
#include < lanelet2_extension/utility/utilities.hpp>
22
23
#include < motion_utils/trajectory/trajectory.hpp>
23
24
#include < tier4_autoware_utils/geometry/boost_polygon_utils.hpp> // for toPolygon2d
@@ -1145,25 +1146,11 @@ void IntersectionModule::reactRTCApproval(
1145
1146
bool IntersectionModule::isGreenSolidOn () const
1146
1147
{
1147
1148
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
1148
- const auto lanelet_map_ptr = planner_data_->route_handler_ ->getLaneletMapPtr ();
1149
- const auto & lane = lanelet_map_ptr->laneletLayer .get (lane_id_);
1150
1149
1151
- std::optional<lanelet::Id> tl_id = std::nullopt;
1152
- for (auto && tl_reg_elem : lane.regulatoryElementsAs <lanelet::TrafficLight>()) {
1153
- tl_id = tl_reg_elem->id ();
1154
- break ;
1155
- }
1156
- if (!tl_id) {
1157
- // this lane has no traffic light
1158
- return false ;
1159
- }
1160
- const auto tl_info_opt = planner_data_->getTrafficSignal (
1161
- tl_id.value (), true /* traffic light module keeps last observation*/ );
1162
- if (!tl_info_opt) {
1163
- // the info of this traffic light is not available
1150
+ if (!last_tl_valid_observation_) {
1164
1151
return false ;
1165
1152
}
1166
- const auto & tl_info = tl_info_opt .value ();
1153
+ const auto & tl_info = last_tl_valid_observation_ .value ();
1167
1154
for (auto && tl_light : tl_info.signal .elements ) {
1168
1155
if (
1169
1156
tl_light.color == TrafficSignalElement::GREEN &&
@@ -1178,38 +1165,88 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori
1178
1165
{
1179
1166
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
1180
1167
1168
+ auto corresponding_arrow = [&](const TrafficSignalElement & element) {
1169
+ if (turn_direction_ == " straight" && element.shape == TrafficSignalElement::UP_ARROW) {
1170
+ return true ;
1171
+ }
1172
+ if (turn_direction_ == " left" && element.shape == TrafficSignalElement::LEFT_ARROW) {
1173
+ return true ;
1174
+ }
1175
+ if (turn_direction_ == " right" && element.shape == TrafficSignalElement::RIGHT_ARROW) {
1176
+ return true ;
1177
+ }
1178
+ return false ;
1179
+ };
1180
+
1181
+ // ==========================================================================================
1182
+ // if no traffic light information has been available, it is UNKNOWN state which is treated as
1183
+ // NOT_PRIORITIZED
1184
+ //
1185
+ // if there has been any information available in the past more than once, the last valid
1186
+ // information is kept and used for planning
1187
+ // ==========================================================================================
1188
+ auto level = TrafficPrioritizedLevel::NOT_PRIORITIZED;
1189
+ if (last_tl_valid_observation_) {
1190
+ auto color = TrafficSignalElement::GREEN;
1191
+ const auto & tl_info = last_tl_valid_observation_.value ();
1192
+ bool has_amber_signal{false };
1193
+ for (auto && tl_light : tl_info.signal .elements ) {
1194
+ if (
1195
+ tl_light.color == TrafficSignalElement::AMBER &&
1196
+ tl_light.shape == TrafficSignalElement::CIRCLE) {
1197
+ has_amber_signal = true ;
1198
+ }
1199
+ if (
1200
+ (tl_light.color == TrafficSignalElement::RED &&
1201
+ tl_light.shape == TrafficSignalElement::CIRCLE) ||
1202
+ (tl_light.color == TrafficSignalElement::GREEN && corresponding_arrow (tl_light))) {
1203
+ // NOTE: Return here since the red signal has the highest priority.
1204
+ level = TrafficPrioritizedLevel::FULLY_PRIORITIZED;
1205
+ color = TrafficSignalElement::RED;
1206
+ break ;
1207
+ }
1208
+ }
1209
+ if (has_amber_signal) {
1210
+ level = TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
1211
+ color = TrafficSignalElement::AMBER;
1212
+ }
1213
+ if (tl_id_and_point_) {
1214
+ const auto [tl_id, point] = tl_id_and_point_.value ();
1215
+ debug_data_.traffic_light_observation =
1216
+ std::make_tuple (planner_data_->current_odometry ->pose , point, tl_id, color);
1217
+ }
1218
+ }
1219
+ return level;
1220
+ }
1221
+
1222
+ void IntersectionModule::updateTrafficSignalObservation ()
1223
+ {
1181
1224
const auto lanelet_map_ptr = planner_data_->route_handler_ ->getLaneletMapPtr ();
1182
1225
const auto & lane = lanelet_map_ptr->laneletLayer .get (lane_id_);
1183
1226
1184
- std::optional<lanelet::Id> tl_id = std::nullopt;
1185
- for (auto && tl_reg_elem : lane.regulatoryElementsAs <lanelet::TrafficLight>()) {
1186
- tl_id = tl_reg_elem->id ();
1187
- break ;
1227
+ if (!tl_id_and_point_) {
1228
+ for (auto && tl_reg_elem :
1229
+ lane.regulatoryElementsAs <lanelet::autoware::AutowareTrafficLight>()) {
1230
+ for (const auto & ls : tl_reg_elem->lightBulbs ()) {
1231
+ if (ls.hasAttribute (" traffic_light_id" )) {
1232
+ tl_id_and_point_ = std::make_pair (tl_reg_elem->id (), ls.front ());
1233
+ break ;
1234
+ }
1235
+ }
1236
+ }
1188
1237
}
1189
- if (!tl_id ) {
1238
+ if (!tl_id_and_point_ ) {
1190
1239
// this lane has no traffic light
1191
- return TrafficPrioritizedLevel::NOT_PRIORITIZED ;
1240
+ return ;
1192
1241
}
1193
- const auto tl_info_opt = planner_data_->getTrafficSignal (
1194
- tl_id.value (), true /* traffic light module keeps last observation*/ );
1242
+ const auto [tl_id, point] = tl_id_and_point_.value ();
1243
+ const auto tl_info_opt =
1244
+ planner_data_->getTrafficSignal (tl_id, true /* traffic light module keeps last observation*/ );
1195
1245
if (!tl_info_opt) {
1196
- return TrafficPrioritizedLevel::NOT_PRIORITIZED;
1197
- }
1198
- const auto & tl_info = tl_info_opt.value ();
1199
- bool has_amber_signal{false };
1200
- for (auto && tl_light : tl_info.signal .elements ) {
1201
- if (tl_light.color == TrafficSignalElement::AMBER) {
1202
- has_amber_signal = true ;
1203
- }
1204
- if (tl_light.color == TrafficSignalElement::RED) {
1205
- // NOTE: Return here since the red signal has the highest priority.
1206
- return TrafficPrioritizedLevel::FULLY_PRIORITIZED;
1207
- }
1208
- }
1209
- if (has_amber_signal) {
1210
- return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
1246
+ // the info of this traffic light is not available
1247
+ return ;
1211
1248
}
1212
- return TrafficPrioritizedLevel::NOT_PRIORITIZED ;
1249
+ last_tl_valid_observation_ = tl_info_opt. value () ;
1213
1250
}
1214
1251
1215
1252
IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus (
0 commit comments