From 4d4abb576c97e278c25e8fb20998428b1d4659e1 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 7 Nov 2023 16:35:38 +0900 Subject: [PATCH 1/2] fix(intersection): reduce path lanelet points (#5507) Signed-off-by: Mamoru Sobue --- .../src/util.cpp | 49 ++++++++++++++++--- .../src/util.hpp | 4 ++ .../utilization/util.hpp | 2 - .../src/utilization/util.cpp | 26 ---------- 4 files changed, 46 insertions(+), 35 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index f605041e22658..d983179d6c5c1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1421,6 +1421,37 @@ static lanelet::ConstLanelets getPrevLanelets( return previous_lanelets; } +// end inclusive +lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, + const double interval) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + if (start_idx < i && i != end_idx) { + const auto & p_prev = path.points.at(i - 1).point.pose; + if (tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + } + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, @@ -1430,6 +1461,8 @@ std::optional generatePathLanelets( const std::vector & attention_areas, const size_t closest_idx, const double width) { + static constexpr double path_lanelet_interval = 1.5; + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; if (!assigned_lane_interval_opt) { return std::nullopt; @@ -1446,13 +1479,13 @@ std::optional generatePathLanelets( const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; if (closest_idx > assigned_lane_start) { path_lanelets.all.push_back( - planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); + generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); } // ego_or_entry2exit const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); path_lanelets.ego_or_entry2exit = - planning_utils::generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width); + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); // next @@ -1461,7 +1494,8 @@ std::optional generatePathLanelets( const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); if (next_lane_interval_opt) { const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = planning_utils::generatePathLanelet(path, next_start, next_end, width); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); path_lanelets.all.push_back(path_lanelets.next.value()); } } @@ -1477,12 +1511,13 @@ std::optional generatePathLanelets( if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = planning_utils::generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width); + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = planning_utils::generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width); + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); } } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 5185fdf3477e4..74b66e4ef54dc 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -154,6 +154,10 @@ double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); +lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, + const double interval); + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index 14c80b59acf5e..85f48af06a04b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -179,8 +179,6 @@ geometry_msgs::msg::Pose getAheadPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & path); Polygon2d generatePathPolygon( const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); double calcJudgeLineDistWithAccLimit( const double velocity, const double max_stop_acceleration, const double delay_response_time); diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index d98e9002b2057..207c6fdef5694 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -287,32 +287,6 @@ Polygon2d generatePathPolygon( return ego_area; } -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width) -{ - lanelet::Points3d lefts; - for (size_t i = start_idx; i <= end_idx; ++i) { - const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); - const double x = path.points.at(i).point.pose.position.x + width / 2 * std::sin(yaw); - const double y = path.points.at(i).point.pose.position.y - width / 2 * std::cos(yaw); - const lanelet::Point3d p(lanelet::InvalId, x, y, path.points.at(i).point.pose.position.z); - lefts.emplace_back(p); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - - lanelet::Points3d rights; - for (size_t i = start_idx; i <= end_idx; ++i) { - const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); - const double x = path.points.at(i).point.pose.position.x - width / 2 * std::sin(yaw); - const double y = path.points.at(i).point.pose.position.y + width / 2 * std::cos(yaw); - const lanelet::Point3d p(lanelet::InvalId, x, y, path.points.at(i).point.pose.position.z); - rights.emplace_back(p); - } - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - geometry_msgs::msg::Pose transformRelCoordinate2D( const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) { From ea105577465f681b6081a176c027a1f7830ebdde Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 8 Nov 2023 14:28:19 +0900 Subject: [PATCH 2/2] fix(intersection): fix wrong resample process (#5516) Signed-off-by: Mamoru Sobue --- .../behavior_velocity_intersection_module/src/util.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index d983179d6c5c1..10db86f1e35c4 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1428,14 +1428,14 @@ lanelet::ConstLanelet generatePathLanelet( { lanelet::Points3d lefts; lanelet::Points3d rights; + size_t prev_idx = start_idx; for (size_t i = start_idx; i <= end_idx; ++i) { const auto & p = path.points.at(i).point.pose; - if (start_idx < i && i != end_idx) { - const auto & p_prev = path.points.at(i - 1).point.pose; - if (tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { - continue; - } + const auto & p_prev = path.points.at(prev_idx).point.pose; + if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; } + prev_idx = i; const double yaw = tf2::getYaw(p.orientation); const double x = p.position.x; const double y = p.position.y;