lane_id to lane_ids

Signed-off-by: Y.Hisaki <[email protected]>

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, lane_id_, 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, lane_id_, -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(), lane_id_, 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,16 +63,23 @@ 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, 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::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();
+    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;
     }
@@ -92,12 +101,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 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, target_lane_id);
+  return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_ids);
 }
 
 template <class T>
@@ -194,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 size_t lane_id, 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 size_t lane_id, 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, lane_id);
+  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

@@ -56,7 +56,7 @@ TEST(findCollisionSegment, nominal)
   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, {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,10 @@
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 
-#include <algorithm>
-#include <vector>
+#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 +49,15 @@ 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
+  auto lane = this->planner_data_->route_handler_->getLaneletsFromId(lane_id_);
+  auto next_lanes = this->planner_data_->route_handler_->getNextLanelets(lane);
+  std::vector<int64_t> search_lane_ids;
+  search_lane_ids.push_back(lane_id_);
+  for (const auto & next_lane : next_lanes) {
+    search_lane_ids.push_back(next_lane.id());
+  }
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    *path, stop_line, lane_id_, 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, lane_id_);
+      arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, {lane_id_});
     if (!collision) {
       continue;
     }