feat(autoware_behavior_velocity_planner_common,autoware_behavior_velocity_stop_line_module): revert pr7710 and Check next_lane_id in createTargetPoint on stop line

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp

@@ -83,7 +83,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
   // Get stop point
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    original_path, stop_line, planner_param_.stop_margin,
+    original_path, stop_line, {lane_id_}, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
   if (!stop_point) {
     return true;
@@ -128,7 +128,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   if (planner_param_.use_dead_line) {
     // Use '-' for margin because it's the backward distance from stop line
     const auto dead_line_point = arc_lane_utils::createTargetPoint(
-      original_path, stop_line, -planner_param_.dead_line_margin,
+      original_path, stop_line, {lane_id_}, -planner_param_.dead_line_margin,
       planner_data_->vehicle_info_.max_longitudinal_offset_m);
 
     if (dead_line_point) {

planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

@@ -125,7 +125,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason
     return true;
   }
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    original_path, stop_line.value(), planner_param_.stop_margin,
+    original_path, stop_line.value(), {lane_id_}, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
   if (!stop_point) {
     setSafe(true);

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp

@@ -21,8 +21,10 @@
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <algorithm>
+#include <cstdint>
 #include <optional>
 #include <utility>
+#include <vector>
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
@@ -31,7 +33,7 @@ namespace autoware::behavior_velocity_planner
 {
 namespace
 {
-geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p)
+inline geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p)
 {
   geometry_msgs::msg::Point geom_p;
   geom_p.x = p.x();
@@ -61,9 +63,27 @@ std::optional<geometry_msgs::msg::Point> checkCollision(
 template <class T>
 std::optional<PathIndexWithPoint> findCollisionSegment(
   const T & path, const geometry_msgs::msg::Point & stop_line_p1,
-  const geometry_msgs::msg::Point & stop_line_p2)
+  const geometry_msgs::msg::Point & stop_line_p2, const std::vector<int64_t> & target_lane_ids)
 {
   for (size_t i = 0; i < path.points.size() - 1; ++i) {
+    const auto & prev_lane_ids = path.points.at(i).lane_ids;
+    const auto & next_lane_ids = path.points.at(i + 1).lane_ids;
+
+    const bool is_target_lane_in_prev_lane = std::any_of(
+      target_lane_ids.begin(), target_lane_ids.end(), [&prev_lane_ids](size_t target_lane_id) {
+        return std::find(prev_lane_ids.begin(), prev_lane_ids.end(), target_lane_id) !=
+               prev_lane_ids.end();
+      });
+    const bool is_target_lane_in_next_lane = std::any_of(
+      target_lane_ids.begin(), target_lane_ids.end(), [&next_lane_ids](size_t target_lane_id) {
+        return std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) !=
+               next_lane_ids.end();
+      });
+
+    if (!is_target_lane_in_prev_lane && !is_target_lane_in_next_lane) {
+      continue;
+    }
+
     const auto & p1 =
       autoware::universe_utils::getPoint(path.points.at(i));  // Point before collision point
     const auto & p2 =
@@ -81,12 +101,12 @@ std::optional<PathIndexWithPoint> findCollisionSegment(
 
 template <class T>
 std::optional<PathIndexWithPoint> findCollisionSegment(
-  const T & path, const LineString2d & stop_line)
+  const T & path, const LineString2d & stop_line, const std::vector<int64_t> & target_lane_ids)
 {
   const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0));
   const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1));
 
-  return findCollisionSegment(path, stop_line_p1, stop_line_p2);
+  return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_ids);
 }
 
 template <class T>
@@ -183,7 +203,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse
 
 std::optional<PathIndexWithPose> createTargetPoint(
   const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
-  const double margin, const double vehicle_offset);
+  const std::vector<int64_t> & lane_ids, const double margin, const double vehicle_offset);
 
 }  // namespace arc_lane_utils
 }  // namespace autoware::behavior_velocity_planner

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp

@@ -26,8 +26,6 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
-#include <algorithm>
-#include <memory>
 #include <utility>
 #include <vector>
 
@@ -109,10 +107,10 @@ std::optional<PathIndexWithOffset> findOffsetSegment(
 
 std::optional<PathIndexWithPose> createTargetPoint(
   const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
-  const double margin, const double vehicle_offset)
+  const std::vector<int64_t> & lane_ids, const double margin, const double vehicle_offset)
 {
   // Find collision segment
-  const auto collision_segment = findCollisionSegment(path, stop_line);
+  const auto collision_segment = findCollisionSegment(path, stop_line, lane_ids);
   if (!collision_segment) {
     // No collision
     return {};

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp

@@ -49,11 +49,14 @@ TEST(findCollisionSegment, nominal)
    *
    **/
   auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6);
+  for (auto & point : path.points) {
+    point.lane_ids.push_back(100);
+  }
 
   LineString2d stop_line;
   stop_line.emplace_back(Point2d(3.5, 3.0));
   stop_line.emplace_back(Point2d(3.5, -3.0));
-  auto segment = arc_lane_utils::findCollisionSegment(path, stop_line);
+  auto segment = arc_lane_utils::findCollisionSegment(path, stop_line, {100});
   EXPECT_EQ(segment->first, static_cast<size_t>(3));
   EXPECT_DOUBLE_EQ(segment->second.x, 3.5);
   EXPECT_DOUBLE_EQ(segment->second.y, 0.0);

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp

@@ -18,12 +18,12 @@
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 
-#include <algorithm>
-#include <vector>
+#include <lanelet2_core/Forward.h>
+
+#include <cstdint>
 
 namespace autoware::behavior_velocity_planner
 {
-namespace bg = boost::geometry;
 
 StopLineModule::StopLineModule(
   const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
@@ -51,8 +51,22 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
     stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length);
 
   // Calculate stop pose and insert index
+
+  // Due to the resampling of PathWithLaneId, lane_id mismatches can occur at intersections near the
+  // ends of lanes. Therefore, the system now accepts the next and previous lane_ids in addition to
+  // the intended lane_id. See more details in the following link:
+  // https://github.com/autowarefoundation/autoware.universe/pull/7896#issue-2395067667
+  auto lane = this->planner_data_->route_handler_->getLaneletsFromId(lane_id_);
+  std::vector<int64_t> search_lane_ids;
+  search_lane_ids.push_back(lane_id_);
+  for (const auto & next_lane : this->planner_data_->route_handler_->getNextLanelets(lane)) {
+    search_lane_ids.push_back(next_lane.id());
+  }
+  for (const auto & prev_lane : this->planner_data_->route_handler_->getPreviousLanelets(lane)) {
+    search_lane_ids.push_back(prev_lane.id());
+  }
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    *path, stop_line, planner_param_.stop_margin,
+    *path, stop_line, search_lane_ids, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
 
   // If no collision found, do nothing

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp

@@ -379,7 +379,7 @@ std::optional<size_t> VirtualTrafficLightModule::getPathIndexOfFirstEndLine()
     end_line_p2.y = end_line.back().y();
 
     const auto collision =
-      arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2);
+      arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, {lane_id_});
     if (!collision) {
       continue;
     }