diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index ca476481ddde5..f30ed9d7a68da 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -320,8 +320,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double ego_vel = planner_data_->current_velocity->twist.linear.x; const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - const std::optional ego_crosswalk_passage_direction = - findEgoPassageDirectionAlongPath(sparse_resample_path); const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position); @@ -357,6 +355,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( }; std::optional> nearest_stop_info; std::vector stop_factor_points; + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); for (const auto & object : object_info_manager_.getObject()) { const auto & collision_point_opt = object.collision_point; if (collision_point_opt) { @@ -618,9 +618,17 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( if (!intersect_pt1 || !intersect_pt2) { return std::nullopt; } - const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; - const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; - const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + + const auto idx1 = intersect_pt1.value().first; + const auto idx2 = intersect_pt2.value().first; + + const auto min_idx = std::min(idx1, idx2); + const auto dist1 = calcSignedArcLength(path.points, min_idx, intersect_pt1.value().second); + const auto dist2 = calcSignedArcLength(path.points, min_idx, intersect_pt2.value().second); + + const auto & front = dist1 > dist2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = dist1 > dist2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); }