diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index cc1cc9f1ea950..4df404a3ccb27 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -34,6 +34,7 @@ using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; +using motion_utils::calcSignedArcLengthPartialSum; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using motion_utils::resamplePath; @@ -731,15 +732,15 @@ Polygon2d CrosswalkModule::getAttentionArea( { const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); + const auto backward_path_length = + calcSignedArcLength(sparse_resample_path.points, size_t(0), ego_pos); + const auto length_sum = calcSignedArcLengthPartialSum( + sparse_resample_path.points, size_t(0), sparse_resample_path.points.size()); Polygon2d attention_area; for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) { - const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; - const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; - const auto front_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position); - const auto back_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position); + const auto front_length = length_sum.at(j) - backward_path_length; + const auto back_length = length_sum.at(j + 1) - backward_path_length; if (back_length < crosswalk_attention_range.first) { continue;