From d6e6ad3e93d377e409cdc3942a5347efa0625009 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 5 Aug 2022 08:05:29 +0900 Subject: [PATCH] feat(behavior_velocity_planner): use sampling functions from motion utils (#1520) --- .../include/utilization/path_utilization.hpp | 2 +- .../src/scene_module/blind_spot/scene.cpp | 2 +- .../src/scene_module/intersection/util.cpp | 4 +- .../scene_no_stopping_area.cpp | 2 +- .../occlusion_spot/scene_occlusion_spot.cpp | 2 +- .../src/utilization/path_utilization.cpp | 82 +------------------ 6 files changed, 9 insertions(+), 85 deletions(-) diff --git a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp index 75adbd4af55f8..fab3c654b205a 100644 --- a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp @@ -27,7 +27,7 @@ namespace behavior_velocity_planner { bool splineInterpolate( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger); + autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); autoware_auto_planning_msgs::msg::Path interpolatePath( const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval = 1.0); diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 324b09b2455f9..543b20f2d8d01 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -213,7 +213,7 @@ bool BlindSpotModule::generateStopLine( /* spline interpolation */ autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; - if (!splineInterpolate(*path, interval, &path_ip, logger_)) { + if (!splineInterpolate(*path, interval, path_ip, logger_)) { return false; } debug_data_.spline_path = path_ip; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 594e2e2e35d23..0d04ecdd4f03e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -189,7 +189,7 @@ bool generateStopLine( /* spline interpolation */ autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; - if (!splineInterpolate(target_path, interval, &path_ip, logger)) { + if (!splineInterpolate(target_path, interval, path_ip, logger)) { return false; } @@ -561,7 +561,7 @@ bool generateStopLineBeforeIntersection( /* spline interpolation */ autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; - if (!splineInterpolate(input_path, interval, &path_ip, logger)) { + if (!splineInterpolate(input_path, interval, path_ip, logger)) { return false; } const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 519f9ddfd4660..cb04dc9da16f9 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -279,7 +279,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( const double interpolation_interval = 0.5; bool is_in_area = false; autoware_auto_planning_msgs::msg::PathWithLaneId interpolated_path; - if (!splineInterpolate(path, interpolation_interval, &interpolated_path, logger_)) { + if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger_)) { return ego_area; } auto & pp = interpolated_path.points; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 8ae23587e0f07..c4df98bb9a0b0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -104,7 +104,7 @@ bool OcclusionSpotModule::modifyPathVelocity( utils::clipPathByLength(*path, clipped_path, param_.detection_area_length); PathWithLaneId path_interpolated; //! never change this interpolation interval(will affect module accuracy) - splineInterpolate(clipped_path, 1.0, &path_interpolated, logger_); + splineInterpolate(clipped_path, 1.0, path_interpolated, logger_); const geometry_msgs::msg::Point start_point = path_interpolated.points.at(0).point.pose.position; const auto offset = motion_utils::calcSignedArcLength( path_interpolated.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr); diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp index 2e514f79cac5a..c763063bd393d 100644 --- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp @@ -37,91 +37,15 @@ namespace behavior_velocity_planner { bool splineInterpolate( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger) + autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) { - *output = input; - - if (input.points.size() <= 1) { + if (input.points.size() < 2) { RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1."); return false; } - static constexpr double ep = 1.0e-8; - - // calc arclength for path - std::vector base_x; - std::vector base_y; - std::vector base_z; - std::vector base_v; - for (const auto & p : input.points) { - base_x.push_back(p.point.pose.position.x); - base_y.push_back(p.point.pose.position.y); - base_z.push_back(p.point.pose.position.z); - base_v.push_back(p.point.longitudinal_velocity_mps); - } - std::vector base_s = calcEuclidDist(base_x, base_y); + output = motion_utils::resamplePath(input, interval, false, true, true, false); - // remove duplicating sample points - { - size_t Ns = base_s.size(); - size_t i = 1; - while (i < Ns) { - if (std::fabs(base_s[i - 1] - base_s[i]) < ep) { - base_s.erase(base_s.begin() + i); - base_x.erase(base_x.begin() + i); - base_y.erase(base_y.begin() + i); - base_z.erase(base_z.begin() + i); - base_v.erase(base_v.begin() + i); - Ns -= 1; - i -= 1; - } - ++i; - } - } - - std::vector resampled_s; - for (double d = 0.0; d < base_s.back() - ep; d += interval) { - resampled_s.push_back(d); - } - - // do spline for xy - const std::vector resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s); - const std::vector resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s); - const std::vector resampled_z = ::interpolation::lerp(base_s, base_z, resampled_s); - const std::vector resampled_v = - ::interpolation::zero_order_hold(base_s, base_v, resampled_s); - - // set xy - output->points.clear(); - for (size_t i = 0; i < resampled_s.size(); i++) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; - p.point.pose.position.x = resampled_x.at(i); - p.point.pose.position.y = resampled_y.at(i); - p.point.pose.position.z = resampled_z.at(i); - p.point.longitudinal_velocity_mps = resampled_v.at(i); - output->points.push_back(p); - } - - // set yaw - for (int i = 1; i < static_cast(resampled_s.size()) - 1; i++) { - auto p = output->points.at(i - 1).point.pose.position; - auto n = output->points.at(i + 1).point.pose.position; - double yaw = std::atan2(n.y - p.y, n.x - p.x); - output->points.at(i).point.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); - } - if (output->points.size() > 1) { - size_t l = output->points.size(); - output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation; - output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation; - } - - // insert final point on path if its position is not same point as interpolated - const auto & op = output->points.back().point.pose.position; - const auto & ip = input.points.back().point.pose.position; - if (std::hypot(op.x - ip.x, op.y - ip.y) > ep) { - output->points.emplace_back(input.points.back()); - } return true; }