Merge pull request #1357 from tier4/beta/v0.11.1-remove-lane_id-from-arc_lane_util

fix(autoware_behavior_velocity_planner_common): remove lane_id check from arc_lane_util

Author: yhisaki

planning/behavior_velocity_detection_area_module/src/scene.cpp

@@ -82,7 +82,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
   // Get stop point
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    original_path, stop_line, lane_id_, planner_param_.stop_margin,
+    original_path, stop_line, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
   if (!stop_point) {
     return true;
@@ -127,7 +127,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, lane_id_, -planner_param_.dead_line_margin,
+      original_path, stop_line, -planner_param_.dead_line_margin,
       planner_data_->vehicle_info_.max_longitudinal_offset_m);
 
     if (dead_line_point) {

planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

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

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp

@@ -126,20 +126,9 @@ inline boost::optional<geometry_msgs::msg::Point> checkCollision(
 template <class T>
 boost::optional<PathIndexWithPoint> findCollisionSegment(
   const T & path, const geometry_msgs::msg::Point & stop_line_p1,
-  const geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id)
+  const geometry_msgs::msg::Point & stop_line_p2)
 {
   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::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::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 =
       tier4_autoware_utils::getPoint(path.points.at(i));  // Point before collision point
     const auto & p2 =
@@ -157,12 +146,12 @@ boost::optional<PathIndexWithPoint> findCollisionSegment(
 
 template <class T>
 boost::optional<PathIndexWithPoint> findCollisionSegment(
-  const T & path, const LineString2d & stop_line, const size_t target_lane_id)
+  const T & path, const LineString2d & stop_line)
 {
   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, target_lane_id);
+  return findCollisionSegment(path, stop_line_p1, stop_line_p2);
 }
 
 template <class T>
@@ -265,10 +254,10 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse
 
 inline boost::optional<PathIndexWithPose> createTargetPoint(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
-  const size_t lane_id, const double margin, const double vehicle_offset)
+  const double margin, const double vehicle_offset)
 {
   // Find collision segment
-  const auto collision_segment = findCollisionSegment(path, stop_line, lane_id);
+  const auto collision_segment = findCollisionSegment(path, stop_line);
   if (!collision_segment) {
     // No collision
     return {};

planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp

@@ -48,14 +48,11 @@ 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, 100);
+  auto segment = arc_lane_utils::findCollisionSegment(path, stop_line);
   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_stop_line_module/src/scene.cpp

@@ -52,7 +52,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
 
   // Calculate stop pose and insert index
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    *path, stop_line, lane_id_, planner_param_.stop_margin,
+    *path, stop_line, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
 
   // If no collision found, do nothing

planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp

@@ -379,7 +379,7 @@ boost::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, lane_id_);
+      arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2);
     if (!collision) {
       continue;
     }