From 8ae43b7d9c486f1f4401153e571ed8e25db37e15 Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Tue, 3 Oct 2023 17:45:12 +0900
Subject: [PATCH] temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../src/scene_crosswalk.cpp                   | 26 +++++++++++++++----
 1 file changed, 21 insertions(+), 5 deletions(-)

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 784896449839a..a23acaa7ba8ea 100644
--- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -232,7 +232,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     getPolygonIntersects(*path, crosswalk_.polygon2d().basicPolygon(), ego_pos, 2);
 
   // TBD
-  std::vector<geometry_msgs::msg::Point> ahead_inserted_stop_point{};
+  std::vector<Point> ahead_inserted_stop_point{};
   if (!path_intersects.empty()) {
     const double ahead_margin = 3.0;
     ahead_inserted_stop_point =
@@ -341,7 +341,7 @@ std::vector<Point> CrosswalkModule::searchAheadInsertedStopPoint(
     if (std::abs(ego_path.points.at(idx).point.longitudinal_velocity_mps) < epsilon) {
       RCLCPP_ERROR(
         logger_, "Stop point is found and distance from original stop point: %f",
-        calcSignedArcLength(ego_path.points, idx, candidate_stop_point_idx));
+        calcSignedArcLength(ego_path.points, candidate_stop_point_idx, idx));
       const auto stop_point = createPoint(
         ego_path.points.at(idx).point.pose.position.x,
         ego_path.points.at(idx).point.pose.position.y,
@@ -457,12 +457,15 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
             << inserted_stop_point_is_front_of_crosswalk << std::endl;
   std::cerr << "stop_at_stop_line:" << stop_at_stop_line << std::endl;
   std::cerr << "default_stop_pose:" << default_stop_pose.has_value() << std::endl;
+  std::cerr << "stop_line_found:" << stop_line_found << std::endl;
   if (stop_at_stop_line) {
     // Stop at the stop line
     if (default_stop_pose) {
       if (!stop_line_found && inserted_stop_point_is_front_of_crosswalk) {
         std::cerr << "\n\n\n\n\n skipped insert stop point \n\n\n\n\n" << std::endl;
-        return {};
+        const auto inserted_stop_pose =
+          calcLongitudinalOffsetPose(ego_path.points, ahead_inserted_stop_point.front(), 0.0);
+        return createStopFactor(*inserted_stop_pose, ahead_inserted_stop_point);
       }
       return createStopFactor(*default_stop_pose, stop_factor_points);
     }
@@ -944,6 +947,11 @@ std::optional<StopFactor> CrosswalkModule::getNearestStopFactor(
     get_distance_to_stop(stop_factor_for_crosswalk_users);
   const auto dist_to_stop_for_stuck_vehicles = get_distance_to_stop(stop_factor_for_stuck_vehicles);
 
+  std::cerr << "dist_to_stop_for_crosswalk_users: " << dist_to_stop_for_crosswalk_users.value_or(-1)
+            << std::endl;
+  std::cerr << "dist_to_stop_for_stuck_vehicles: " << dist_to_stop_for_stuck_vehicles.value_or(-1)
+            << std::endl;
+
   if (dist_to_stop_for_crosswalk_users) {
     if (dist_to_stop_for_stuck_vehicles) {
       if (*dist_to_stop_for_stuck_vehicles < *dist_to_stop_for_crosswalk_users) {
@@ -1140,8 +1148,16 @@ void CrosswalkModule::setDistanceToStop(
 {
   // calculate stop position
   const auto stop_pos = [&]() -> std::optional<geometry_msgs::msg::Point> {
-    if (stop_factor) return stop_factor->stop_pose.position;
-    if (default_stop_pose) return default_stop_pose->position;
+    if (stop_factor) {
+      std::cerr << "stop_factor is not null" << std::endl;
+      return stop_factor->stop_pose.position;
+    }
+
+    if (default_stop_pose) {
+      std::cerr << "default_sto_pose is not null" << std::endl;
+      return default_stop_pose->position;
+    }
+    std::cerr << "stop_factor and default_stop_pose are null" << std::endl;
     return std::nullopt;
   }();