fix(autoware_behavior_velocity_planner_common): remove lane_id check …

…from arc_lane_util (autowarefoundation#7710) * fix(arc_lane_util): remove lane_id check from arc_lane_util Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * modify test_arc_lane_util.cpp Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> --------- Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
tier4 · Jun 27, 2024 · be02771 · be02771
1 parent c129187
commit be02771
Show file tree

Hide file tree

Showing 7 changed files with 12 additions and 26 deletions.
diff --git a/.../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/.../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, 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;
@@ -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, 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) {

diff --git a/...planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/...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(), 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);

diff --git a/...er_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/...er_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp
@@ -61,20 +61,9 @@ 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 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 =
       autoware::universe_utils::getPoint(path.points.at(i));  // Point before collision point
     const auto & p2 =
@@ -92,12 +81,12 @@ std::optional<PathIndexWithPoint> findCollisionSegment(
 
 template <class T>
 std::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>
@@ -194,7 +183,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 size_t lane_id, const double margin, const double vehicle_offset);
+  const double margin, const double vehicle_offset);
 
 }  // namespace arc_lane_utils
 }  // namespace autoware::behavior_velocity_planner

diff --git a/...ocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/...ocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp
@@ -109,10 +109,10 @@ std::optional<PathIndexWithOffset> findOffsetSegment(
 
 std::optional<PathIndexWithPose> createTargetPoint(
   const tier4_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 {};

diff --git a/...elocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/...elocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp
@@ -49,14 +49,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);

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_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

diff --git a/...or_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/...or_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, lane_id_);
+      arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2);
     if (!collision) {
       continue;
     }