From 89dbb5fccaeac645e3feea99752fcc948e4f0555 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 16:27:46 +0900 Subject: [PATCH 1/3] Revert "fix(autoware_behavior_velocity_planner_common): remove lane_id check from arc_lane_util (#7710)" This reverts commit be0277126343fce40bc00a8dbbe56ba93dd6897d. --- .../src/scene.cpp | 4 ++-- .../src/scene_no_stopping_area.cpp | 2 +- .../utilization/arc_lane_util.hpp | 19 +++++++++++++++---- .../src/utilization/arc_lane_util.cpp | 4 ++-- .../test/src/test_arc_lane_util.cpp | 5 ++++- .../src/scene.cpp | 2 +- .../src/scene.cpp | 2 +- 7 files changed, 26 insertions(+), 12 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 9cbe962e88cae..7abd88d59f398 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, 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, -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 0478aea34a44f..d007416dccb47 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(), 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 769932259e3e6..77b7d25f9f46e 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 @@ -61,9 +61,20 @@ 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 geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id) { 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 = @@ -81,12 +92,12 @@ std::optional findCollisionSegment( template std::optional findCollisionSegment( - const T & path, const LineString2d & stop_line) + const T & path, const LineString2d & stop_line, const size_t target_lane_id) { 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); + return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_id); } template @@ -183,7 +194,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 double margin, const double vehicle_offset); + const size_t lane_id, 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 da6ef7262de74..3e029174b288d 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 @@ -109,10 +109,10 @@ std::optional findOffsetSegment( std::optional createTargetPoint( const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const double margin, const double vehicle_offset) + const size_t lane_id, const double margin, const double vehicle_offset) { // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line); + const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); 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 db9a63169e565..ddedec80dc0f3 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 @@ -49,11 +49,14 @@ 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); + 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 04ea5ca872098..6075706a672d0 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 @@ -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, planner_param_.stop_margin, + *path, stop_line, lane_id_, 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 1bf26d9f95c6a..d27cc23223630 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); + arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, lane_id_); if (!collision) { continue; } From 44be7eefede15a58ca2c311d170bab78a05f8c1c Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 17:48:27 +0900 Subject: [PATCH 2/3] 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; } From 72c598568b89cd2da2f37190abdbd72dbe9c068f Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 20:36:06 +0900 Subject: [PATCH 3/3] add detail comment and check prev lane Signed-off-by: Y.Hisaki --- .../src/scene.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) 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 0ec2fe861941b..0f90c05131a18 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,6 +18,8 @@ #include #include +#include + #include namespace autoware::behavior_velocity_planner @@ -49,13 +51,20 @@ 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 + + // Due to the resampling of PathWithLaneId, lane_id mismatches can occur at intersections near the + // ends of lanes. Therefore, the system now accepts the next and previous lane_ids in addition to + // the intended lane_id. See more details in the following link: + // https://github.com/autowarefoundation/autoware.universe/pull/7896#issue-2395067667 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) { + for (const auto & next_lane : this->planner_data_->route_handler_->getNextLanelets(lane)) { search_lane_ids.push_back(next_lane.id()); } + for (const auto & prev_lane : this->planner_data_->route_handler_->getPreviousLanelets(lane)) { + search_lane_ids.push_back(prev_lane.id()); + } const auto stop_point = arc_lane_utils::createTargetPoint( *path, stop_line, search_lane_ids, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m);