Skip to content

Commit a45f52a

Browse files
authored
Merge pull request #1091 from tier4/cherry-pick/intersection
feat(intersection): cherry pick awf autowarefoundation#6036, autowarefoundation#6042, autowarefoundation#6050
2 parents d8f135f + e0f4760 commit a45f52a

File tree

9 files changed

+223
-72
lines changed

9 files changed

+223
-72
lines changed

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -1055,19 +1055,20 @@ bool CrosswalkModule::isRedSignalForPedestrians() const
10551055
crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>();
10561056

10571057
for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) {
1058-
const auto traffic_signal_stamped =
1058+
const auto traffic_signal_stamped_opt =
10591059
planner_data_->getTrafficSignal(traffic_lights_reg_elem->id());
1060-
if (!traffic_signal_stamped) {
1060+
if (!traffic_signal_stamped_opt) {
10611061
continue;
10621062
}
1063+
const auto traffic_signal_stamped = traffic_signal_stamped_opt.value();
10631064

10641065
if (
10651066
planner_param_.traffic_light_state_timeout <
1066-
(clock_->now() - traffic_signal_stamped->stamp).seconds()) {
1067+
(clock_->now() - traffic_signal_stamped.stamp).seconds()) {
10671068
continue;
10681069
}
10691070

1070-
const auto & lights = traffic_signal_stamped->signal.elements;
1071+
const auto & lights = traffic_signal_stamped.signal.elements;
10711072
if (lights.empty()) {
10721073
continue;
10731074
}

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,7 @@
7272
enable: false
7373
creep_velocity: 0.8333
7474
peeking_offset: -0.5
75-
occlusion_required_clearance_distance: 55
75+
occlusion_required_clearance_distance: 55.0
7676
possible_object_bbox: [1.5, 2.5]
7777
ignore_parked_vehicle_speed_threshold: 0.8333
7878
occlusion_detection_hold_time: 1.5

planning/behavior_velocity_intersection_module/src/debug.cpp

+16
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
188188
&debug_marker_array);
189189
}
190190

191+
if (debug_data_.first_attention_area) {
192+
appendMarkerArray(
193+
createLaneletPolygonsMarkerArray(
194+
{debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647,
195+
0.0),
196+
&debug_marker_array, now);
197+
}
198+
199+
if (debug_data_.second_attention_area) {
200+
appendMarkerArray(
201+
createLaneletPolygonsMarkerArray(
202+
{debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647,
203+
0.0),
204+
&debug_marker_array, now);
205+
}
206+
191207
if (debug_data_.stuck_vehicle_detect_area) {
192208
appendMarkerArray(
193209
debug::createPolygonMarkerArray(

planning/behavior_velocity_intersection_module/src/manager.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -152,6 +152,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
152152
getOrDeclareParameter<double>(node, ns + ".occlusion.creep_during_peeking.creep_velocity");
153153
ip.occlusion.peeking_offset =
154154
getOrDeclareParameter<double>(node, ns + ".occlusion.peeking_offset");
155+
ip.occlusion.occlusion_required_clearance_distance =
156+
getOrDeclareParameter<double>(node, ns + ".occlusion.occlusion_required_clearance_distance");
155157
ip.occlusion.possible_object_bbox =
156158
getOrDeclareParameter<std::vector<double>>(node, ns + ".occlusion.possible_object_bbox");
157159
ip.occlusion.ignore_parked_vehicle_speed_threshold =

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+120-50
Large diffs are not rendered by default.

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

+45-8
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,8 @@ struct DebugData
5353
std::optional<std::vector<lanelet::CompoundPolygon3d>> occlusion_attention_area{std::nullopt};
5454
std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
5555
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};
5658
std::optional<geometry_msgs::msg::Polygon> stuck_vehicle_detect_area{std::nullopt};
5759
std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_stuck_detect_area{std::nullopt};
5860
std::optional<geometry_msgs::msg::Polygon> candidate_collision_ego_lane_polygon{std::nullopt};
@@ -82,7 +84,8 @@ struct IntersectionLanelets
8284
*/
8385
void update(
8486
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);
8689

8790
const lanelet::ConstLanelets & attention() const
8891
{
@@ -131,6 +134,14 @@ struct IntersectionLanelets
131134
{
132135
return first_attention_area_;
133136
}
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+
}
134145

135146
/**
136147
* the set of attention lanelets which is topologically merged
@@ -178,17 +189,25 @@ struct IntersectionLanelets
178189
std::vector<double> occlusion_attention_size_;
179190

180191
/**
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
182193
*/
183194
std::optional<lanelet::ConstLanelet> first_conflicting_lane_{std::nullopt};
184195
std::optional<lanelet::CompoundPolygon3d> first_conflicting_area_{std::nullopt};
185196

186197
/**
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
188199
*/
189200
std::optional<lanelet::ConstLanelet> first_attention_lane_{std::nullopt};
190201
std::optional<lanelet::CompoundPolygon3d> first_attention_area_{std::nullopt};
191202

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+
192211
/**
193212
* flag if the intersection is prioritized by the traffic light
194213
*/
@@ -219,16 +238,28 @@ struct IntersectionStopLines
219238
*/
220239
std::optional<size_t> first_attention_stopline{std::nullopt};
221240

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+
222247
/**
223248
* occlusion_peeking_stopline is null if path[0] is already inside the attention area
224249
*/
225250
std::optional<size_t> occlusion_peeking_stopline{std::nullopt};
226251

227252
/**
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
230261
*/
231-
size_t pass_judge_line{0};
262+
size_t second_pass_judge_line{0};
232263

233264
/**
234265
* 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
631662
* @fn
632663
* @brief find TrafficPrioritizedLevel
633664
*/
634-
TrafficPrioritizedLevel getTrafficPrioritizedLevel(
635-
lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos);
665+
TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane);
636666

637667
/**
638668
* @fn
@@ -651,6 +681,7 @@ class IntersectionModule : public SceneModuleInterface
651681
lanelet::ConstLanelet assigned_lanelet,
652682
const lanelet::CompoundPolygon3d & first_conflicting_area,
653683
const lanelet::ConstLanelet & first_attention_lane,
684+
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
654685
const util::InterpolatedPathInfo & interpolated_path_info,
655686
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
656687

@@ -738,6 +769,12 @@ class IntersectionModule : public SceneModuleInterface
738769
const std::vector<lanelet::ConstLineString3d> & lane_divisions,
739770
const TargetObjects & target_objects);
740771

772+
/*
773+
* @fn
774+
* @brief check if associated traffic light is green
775+
*/
776+
bool isGreenSolidOn(lanelet::ConstLanelet lane);
777+
741778
/*
742779
bool IntersectionModule::checkFrontVehicleDeceleration(
743780
lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet,

planning/behavior_velocity_planner/src/node.cpp

+15-1
Original file line numberDiff line numberDiff line change
@@ -327,7 +327,21 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
327327
TrafficSignalStamped traffic_signal;
328328
traffic_signal.stamp = msg->stamp;
329329
traffic_signal.signal = signal;
330-
planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal;
330+
planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal;
331+
const bool is_unknown_observation =
332+
std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) {
333+
return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN;
334+
});
335+
// if the observation is UNKNOWN and past observation is available, only update the timestamp
336+
// and keep the body of the info
337+
if (
338+
is_unknown_observation &&
339+
planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) {
340+
planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp =
341+
msg->stamp;
342+
} else {
343+
planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal;
344+
}
331345
}
332346
}
333347

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

+15-4
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,10 @@ struct PlannerData
7676
double ego_nearest_yaw_threshold;
7777

7878
// other internal data
79-
std::map<int, TrafficSignalStamped> traffic_light_id_map;
79+
// traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the
80+
// last observed infomation for UNKNOWN
81+
std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
82+
std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
8083
std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
8184
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
8285

@@ -125,12 +128,20 @@ struct PlannerData
125128
return true;
126129
}
127130

128-
std::shared_ptr<TrafficSignalStamped> getTrafficSignal(const int id) const
131+
/**
132+
*@fn
133+
*@brief queries the traffic signal information of given Id. if keep_last_observation = true,
134+
*recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation
135+
*/
136+
std::optional<TrafficSignalStamped> getTrafficSignal(
137+
const lanelet::Id id, const bool keep_last_observation = false) const
129138
{
139+
const auto & traffic_light_id_map =
140+
keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_;
130141
if (traffic_light_id_map.count(id) == 0) {
131-
return {};
142+
return std::nullopt;
132143
}
133-
return std::make_shared<TrafficSignalStamped>(traffic_light_id_map.at(id));
144+
return std::make_optional<TrafficSignalStamped>(traffic_light_id_map.at(id));
134145
}
135146
};
136147
} // namespace behavior_velocity_planner

planning/behavior_velocity_traffic_light_module/src/scene.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -353,15 +353,15 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
353353
bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const
354354
{
355355
// get traffic signal associated with the regulatory element id
356-
const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id());
357-
if (!traffic_signal_stamped) {
356+
const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal(
357+
traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */);
358+
if (!traffic_signal_stamped_opt) {
358359
RCLCPP_WARN_THROTTLE(
359360
logger_, *clock_, 5000 /* ms */,
360361
"the traffic signal data associated with regulatory element id is not received");
361362
return false;
362363
}
363-
364-
valid_traffic_signal = *traffic_signal_stamped;
364+
valid_traffic_signal = traffic_signal_stamped_opt.value();
365365
return true;
366366
}
367367

0 commit comments

Comments
 (0)