Skip to content

Commit c1b8f46

Browse files
authored
fix(avoidance): ignore objects are in intersection when the ego stops by red traffic signal (#6565)
fix(avoidance): don't avoid objects are farther than red signal traffic light Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 5adc394 commit c1b8f46

File tree

1 file changed

+10
-1
lines changed
  • planning/behavior_path_avoidance_module/src

1 file changed

+10
-1
lines changed

planning/behavior_path_avoidance_module/src/scene.cpp

+10-1
Original file line numberDiff line numberDiff line change
@@ -321,10 +321,19 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
321321
using utils::avoidance::getTargetLanelets;
322322
using utils::avoidance::separateObjectsByPath;
323323
using utils::avoidance::updateRoadShoulderDistance;
324+
using utils::traffic_light::calcDistanceToRedTrafficLight;
324325

325326
// Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
326327
constexpr double MARGIN = 10.0;
327-
const auto forward_detection_range = helper_->getForwardDetectionRange();
328+
const auto forward_detection_range = [&]() {
329+
const auto to_traffic_light = calcDistanceToRedTrafficLight(
330+
data.current_lanelets, helper_->getPreviousReferencePath(), planner_data_);
331+
if (!to_traffic_light.has_value()) {
332+
return helper_->getForwardDetectionRange();
333+
}
334+
return std::min(helper_->getForwardDetectionRange(), to_traffic_light.value());
335+
}();
336+
328337
const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath(
329338
helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_,
330339
data, parameters_, forward_detection_range + MARGIN, debug);

0 commit comments

Comments
 (0)