@@ -53,6 +53,8 @@ struct DebugData
53
53
std::optional<std::vector<lanelet::CompoundPolygon3d>> occlusion_attention_area{std::nullopt};
54
54
std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
55
55
std::optional<std::vector<lanelet::CompoundPolygon3d>> adjacent_area{std::nullopt};
56
+ std::optional<lanelet::CompoundPolygon3d> first_attention_area{std::nullopt};
57
+ std::optional<lanelet::CompoundPolygon3d> second_attention_area{std::nullopt};
56
58
std::optional<geometry_msgs::msg::Polygon> stuck_vehicle_detect_area{std::nullopt};
57
59
std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_stuck_detect_area{std::nullopt};
58
60
std::optional<geometry_msgs::msg::Polygon> candidate_collision_ego_lane_polygon{std::nullopt};
@@ -82,7 +84,8 @@ struct IntersectionLanelets
82
84
*/
83
85
void update (
84
86
const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info,
85
- const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length);
87
+ const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length,
88
+ lanelet::routing::RoutingGraphPtr routing_graph_ptr);
86
89
87
90
const lanelet::ConstLanelets & attention () const
88
91
{
@@ -131,6 +134,14 @@ struct IntersectionLanelets
131
134
{
132
135
return first_attention_area_;
133
136
}
137
+ const std::optional<lanelet::ConstLanelet> & second_attention_lane () const
138
+ {
139
+ return second_attention_lane_;
140
+ }
141
+ const std::optional<lanelet::CompoundPolygon3d> & second_attention_area () const
142
+ {
143
+ return second_attention_area_;
144
+ }
134
145
135
146
/* *
136
147
* the set of attention lanelets which is topologically merged
@@ -178,17 +189,25 @@ struct IntersectionLanelets
178
189
std::vector<double > occlusion_attention_size_;
179
190
180
191
/* *
181
- * the attention lanelet which ego path points intersect for the first time
192
+ * the first conflicting lanelet which ego path points intersect for the first time
182
193
*/
183
194
std::optional<lanelet::ConstLanelet> first_conflicting_lane_{std::nullopt};
184
195
std::optional<lanelet::CompoundPolygon3d> first_conflicting_area_{std::nullopt};
185
196
186
197
/* *
187
- * the attention lanelet which ego path points intersect for the first time
198
+ * the first attention lanelet which ego path points intersect for the first time
188
199
*/
189
200
std::optional<lanelet::ConstLanelet> first_attention_lane_{std::nullopt};
190
201
std::optional<lanelet::CompoundPolygon3d> first_attention_area_{std::nullopt};
191
202
203
+ /* *
204
+ * the second attention lanelet which ego path points intersect next to the
205
+ * first_attention_lanelet
206
+ */
207
+ bool second_attention_lane_empty_{false };
208
+ std::optional<lanelet::ConstLanelet> second_attention_lane_{std::nullopt};
209
+ std::optional<lanelet::CompoundPolygon3d> second_attention_area_{std::nullopt};
210
+
192
211
/* *
193
212
* flag if the intersection is prioritized by the traffic light
194
213
*/
@@ -219,16 +238,28 @@ struct IntersectionStopLines
219
238
*/
220
239
std::optional<size_t > first_attention_stopline{std::nullopt};
221
240
241
+ /* *
242
+ * second_attention_stopline is null if ego footprint along the path does not intersect with
243
+ * second_attention_lane. if path[0] satisfies the condition, it is 0
244
+ */
245
+ std::optional<size_t > second_attention_stopline{std::nullopt};
246
+
222
247
/* *
223
248
* occlusion_peeking_stopline is null if path[0] is already inside the attention area
224
249
*/
225
250
std::optional<size_t > occlusion_peeking_stopline{std::nullopt};
226
251
227
252
/* *
228
- * pass_judge_line is before first_attention_stop_line by the braking distance. if its value is
229
- * calculated negative, it is 0
253
+ * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value
254
+ * is calculated negative, it is 0
255
+ */
256
+ size_t first_pass_judge_line{0 };
257
+
258
+ /* *
259
+ * second_pass_judge_line is before second_attention_stopline by the braking distance. if its
260
+ * value is calculated negative, it is 0
230
261
*/
231
- size_t pass_judge_line {0 };
262
+ size_t second_pass_judge_line {0 };
232
263
233
264
/* *
234
265
* occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with
@@ -631,8 +662,7 @@ class IntersectionModule : public SceneModuleInterface
631
662
* @fn
632
663
* @brief find TrafficPrioritizedLevel
633
664
*/
634
- TrafficPrioritizedLevel getTrafficPrioritizedLevel (
635
- lanelet::ConstLanelet lane, const std::map<int , TrafficSignalStamped> & tl_infos);
665
+ TrafficPrioritizedLevel getTrafficPrioritizedLevel (lanelet::ConstLanelet lane);
636
666
637
667
/* *
638
668
* @fn
@@ -651,6 +681,7 @@ class IntersectionModule : public SceneModuleInterface
651
681
lanelet::ConstLanelet assigned_lanelet,
652
682
const lanelet::CompoundPolygon3d & first_conflicting_area,
653
683
const lanelet::ConstLanelet & first_attention_lane,
684
+ const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
654
685
const util::InterpolatedPathInfo & interpolated_path_info,
655
686
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
656
687
@@ -738,6 +769,12 @@ class IntersectionModule : public SceneModuleInterface
738
769
const std::vector<lanelet::ConstLineString3d> & lane_divisions,
739
770
const TargetObjects & target_objects);
740
771
772
+ /*
773
+ * @fn
774
+ * @brief check if associated traffic light is green
775
+ */
776
+ bool isGreenSolidOn (lanelet::ConstLanelet lane);
777
+
741
778
/*
742
779
bool IntersectionModule::checkFrontVehicleDeceleration(
743
780
lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet,
0 commit comments