From 293f804e357d7c8a9353ee3bc40ec1e648a12381 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 17:48:27 +0900 Subject: [PATCH] lane_id to lane_ids Signed-off-by: Y.Hisaki --- .../src/scene.cpp | 4 +-- .../src/scene_no_stopping_area.cpp | 2 +- .../utilization/arc_lane_util.hpp | 27 ++++++++++++------- .../src/utilization/arc_lane_util.cpp | 6 ++--- .../test/src/test_arc_lane_util.cpp | 2 +- .../src/scene.cpp | 13 ++++++--- .../src/scene.cpp | 2 +- 7 files changed, 34 insertions(+), 22 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 7abd88d59f398..57aeac11c9aed 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/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) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index d007416dccb47..4d800b2926eb7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/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); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 77b7d25f9f46e..178fca4ce046f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/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 #include +#include #include #include +#include #define EIGEN_MPL2_ONLY #include @@ -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 checkCollision( template std::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, const std::vector & 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 findCollisionSegment( template std::optional findCollisionSegment( - const T & path, const LineString2d & stop_line, const size_t target_lane_id) + const T & path, const LineString2d & stop_line, const std::vector & 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 @@ -194,7 +203,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse std::optional 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 & lane_ids, const double margin, const double vehicle_offset); } // namespace arc_lane_utils } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 3e029174b288d..21104fc022876 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -26,8 +26,6 @@ #include #endif -#include -#include #include #include @@ -109,10 +107,10 @@ std::optional findOffsetSegment( std::optional 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 & 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 {}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index ddedec80dc0f3..223d1f0d83e28 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/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(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 index 6075706a672d0..0ec2fe861941b 100644 --- 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 @@ -18,12 +18,10 @@ #include #include -#include -#include +#include 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 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 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index d27cc23223630..8213b862b89e6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -379,7 +379,7 @@ std::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, {lane_id_}); if (!collision) { continue; }