fix(crosswalk): fix findEgoPassageDirectionAlongPath finding front an…

…d back point logic (#8459) * fix(crosswalk): fix findEgoPassageDirectionAlongPath finding front and back point logic Signed-off-by: Mehmet Dogru <[email protected]> * define ego_crosswalk_passage_direction later Signed-off-by: Mehmet Dogru <[email protected]> --------- Signed-off-by: Mehmet Dogru <[email protected]>
autowarefoundation · Aug 23, 2024 · a6e0ca7 · a6e0ca7
1 parent 23e13fd
commit a6e0ca7
Showing 1 changed file with 13 additions and 5 deletions.
diff --git a/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -320,8 +320,6 @@ std::optional<StopFactor> 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<double> 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<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   };
   std::optional<std::pair<geometry_msgs::msg::Point, double>> nearest_stop_info;
   std::vector<geometry_msgs::msg::Point> stop_factor_points;
+  const std::optional<double> 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<double> 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);
 }