diff --git a/planning/behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_detection_area_module/src/scene.cpp index 53a6deb6f26a4..675e5284790ff 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.cpp +++ b/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) { diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 77fbb18be2b5b..4934755b7fb72 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/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); diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index ffc216e863500..662d9068eb8e5 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -126,20 +126,9 @@ inline boost::optional checkCollision( template boost::optional 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 findCollisionSegment( template boost::optional 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 @@ -265,10 +254,10 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse inline boost::optional 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 {}; diff --git a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index c4fdb83e00609..953699c80485c 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/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(3)); EXPECT_DOUBLE_EQ(segment->second.x, 3.5); EXPECT_DOUBLE_EQ(segment->second.y, 0.0); diff --git a/planning/behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_stop_line_module/src/scene.cpp index a703f8b5566ad..6ef083cfa57d1 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.cpp +++ b/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 diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp index fd71635873e7f..34df4d3d6c7d6 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -379,7 +379,7 @@ boost::optional 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; }