From 245e388ec8b0c2a3b21ed182dd443d671a745687 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 4 Sep 2023 20:27:38 +0900 Subject: [PATCH] fix(crosswalk): set large minus value in rtc distance if the ego already passed crosswalk (#4869) Signed-off-by: satoshi-ota --- .../src/scene_crosswalk.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 05d79420204cf..cc1cc9f1ea950 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -225,9 +225,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Calculate stop point with margin const auto p_stop_line = getStopPointWithMargin(*path, path_intersects); - // TODO(murooka) add a guard of p_stop_line - const auto default_stop_pose = toStdOptional( - calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + + std::optional default_stop_pose = std::nullopt; + if (p_stop_line.has_value()) { + default_stop_pose = toStdOptional( + calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + } // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -992,6 +995,8 @@ void CrosswalkModule::setDistanceToStop( const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); setDistance(dist_ego2stop); + } else { + setDistance(std::numeric_limits::lowest()); } }