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
@@ -1143,25 +1144,11 @@ void IntersectionModule::reactRTCApproval(
1143
1144
bool IntersectionModule::isGreenSolidOn () const
1144
1145
{
1145
1146
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_);
1148
1147
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_) {
1156
1149
return false ;
1157
1150
}
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 ();
1165
1152
for (auto && tl_light : tl_info.signal .elements ) {
1166
1153
if (
1167
1154
tl_light.color == TrafficSignalElement::GREEN &&
@@ -1176,38 +1163,81 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori
1176
1163
{
1177
1164
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
1178
1165
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
+ {
1179
1214
const auto lanelet_map_ptr = planner_data_->route_handler_ ->getLaneletMapPtr ();
1180
1215
const auto & lane = lanelet_map_ptr->laneletLayer .get (lane_id_);
1181
1216
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
+ }
1186
1227
}
1187
- if (!tl_id ) {
1228
+ if (!tl_id_and_point_ ) {
1188
1229
// this lane has no traffic light
1189
- return TrafficPrioritizedLevel::NOT_PRIORITIZED ;
1230
+ return ;
1190
1231
}
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*/ );
1193
1235
if (!tl_info_opt) {
1194
- return TrafficPrioritizedLevel::NOT_PRIORITIZED;
1236
+ // the info of this traffic light is not available
1237
+ return ;
1195
1238
}
1196
1239
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;
1211
1241
}
1212
1242
1213
1243
IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus (
0 commit comments