Skip to content

Commit d1a77d5

Browse files
soblinsoblin
authored
feat(intersection): ignore decelerating vehicle on amber traffic light (#5304)
* merge attention area lanelets topologically Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * query stop line for each merged detection lanelet Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * first conflicting/attention area Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * ignore expectedToStopBeforeStopLine vehicle Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * ignore expectedToStopBeforeStopLine vehicle Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fix Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 4383d6b commit d1a77d5

File tree

8 files changed

+455
-314
lines changed

8 files changed

+455
-314
lines changed

‎planning/behavior_velocity_intersection_module/config/intersection.param.yaml

+5-3
Original file line numberDiff line numberDiff line change
@@ -48,9 +48,11 @@
4848
use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
4949
minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity
5050
yield_on_green_traffic_light:
51-
distance_to_assigned_lanelet_start: 10.0
52-
duration: 3.0
53-
range: 50.0 # [m]
51+
distance_to_assigned_lanelet_start: 5.0
52+
duration: 2.0
53+
object_dist_to_stopline: 10.0 # [m]
54+
ignore_on_amber_traffic_light:
55+
object_expected_deceleration: 2.0 # [m/ss]
5456

5557
occlusion:
5658
enable: false

‎planning/behavior_velocity_intersection_module/src/debug.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -233,6 +233,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
233233
debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0),
234234
&debug_marker_array, now);
235235

236+
appendMarkerArray(
237+
debug::createObjectsMarkerArray(
238+
debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0),
239+
&debug_marker_array, now);
240+
236241
appendMarkerArray(
237242
debug::createObjectsMarkerArray(
238243
debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2),

‎planning/behavior_velocity_intersection_module/src/manager.cpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -129,8 +129,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
129129
ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start");
130130
ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter<double>(
131131
node, ns + ".collision_detection.yield_on_green_traffic_light.duration");
132-
ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter<double>(
133-
node, ns + ".collision_detection.yield_on_green_traffic_light.range");
132+
ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline =
133+
getOrDeclareParameter<double>(
134+
node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline");
135+
ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration =
136+
getOrDeclareParameter<double>(
137+
node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration");
134138

135139
ip.occlusion.enable = getOrDeclareParameter<bool>(node, ns + ".occlusion.enable");
136140
ip.occlusion.occlusion_attention_area_length =

‎planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+172-142
Large diffs are not rendered by default.

‎planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

+14-12
Original file line numberDiff line numberDiff line change
@@ -109,8 +109,12 @@ class IntersectionModule : public SceneModuleInterface
109109
{
110110
double distance_to_assigned_lanelet_start;
111111
double duration;
112-
double range;
112+
double object_dist_to_stopline;
113113
} yield_on_green_traffic_light;
114+
struct IgnoreOnAmberTrafficLight
115+
{
116+
double object_expected_deceleration;
117+
} ignore_on_amber_traffic_light;
114118
} collision_detection;
115119
struct Occlusion
116120
{
@@ -266,7 +270,8 @@ class IntersectionModule : public SceneModuleInterface
266270

267271
// for occlusion detection
268272
const bool enable_occlusion_detection_;
269-
std::optional<std::vector<util::DiscretizedLane>> occlusion_attention_divisions_{std::nullopt};
273+
std::optional<std::vector<lanelet::ConstLineString3d>> occlusion_attention_divisions_{
274+
std::nullopt};
270275
StateMachine collision_state_machine_; //! for stable collision checking
271276
StateMachine before_creep_state_machine_; //! for two phase stop
272277
StateMachine occlusion_stop_state_machine_;
@@ -302,27 +307,24 @@ class IntersectionModule : public SceneModuleInterface
302307
const util::PathLanelets & path_lanelets,
303308
const std::optional<lanelet::CompoundPolygon3d> & first_attention_area);
304309

305-
autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects(
306-
const lanelet::ConstLanelets & attention_area_lanelets,
307-
const lanelet::ConstLanelets & adjacent_lanelets,
310+
util::TargetObjects generateTargetObjects(
311+
const util::IntersectionLanelets & intersection_lanelets,
308312
const std::optional<Polygon2d> & intersection_area) const;
309313

310314
bool checkCollision(
311315
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
312-
const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects,
313-
const util::PathLanelets & path_lanelets, const size_t closest_idx,
314-
const size_t last_intersection_stop_line_candidate_idx, const double time_delay,
315-
const util::TrafficPrioritizedLevel & traffic_prioritized_level);
316+
util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
317+
const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
318+
const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level);
316319

317320
bool isOcclusionCleared(
318321
const nav_msgs::msg::OccupancyGrid & occ_grid,
319322
const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
320323
const lanelet::ConstLanelets & adjacent_lanelets,
321324
const lanelet::CompoundPolygon3d & first_attention_area,
322325
const util::InterpolatedPathInfo & interpolated_path_info,
323-
const std::vector<util::DiscretizedLane> & lane_divisions,
324-
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
325-
parked_attention_objects,
326+
const std::vector<lanelet::ConstLineString3d> & lane_divisions,
327+
const std::vector<util::TargetObject> & blocking_attention_objects,
326328
const double occlusion_dist_thr);
327329

328330
/*

0 commit comments

Comments
 (0)