From 5f86e26d4616d33a88680b1f077029071fc42b1c Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 9 May 2022 10:25:46 +0900 Subject: [PATCH] feat: add angle limitation to mpt (#819) * feat: add angle limitation to mpt Signed-off-by: tomoya.kimura * findNearestIndex -> findNearestIndexWithSoftYawConstraints Signed-off-by: tomoya.kimura * consider yaw-angle to find nearest segment index Signed-off-by: tomoya.kimura * fix typo Signed-off-by: tomoya.kimura * fix pre-commit Signed-off-by: tomoya.kimura --- .../mpt_optimizer.hpp | 7 +- .../obstacle_avoidance_planner/utils.hpp | 12 +++- .../src/mpt_optimizer.cpp | 71 ++++++++++++++----- .../obstacle_avoidance_planner/src/node.cpp | 16 +++-- 4 files changed, 81 insertions(+), 25 deletions(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index f7ff8c9f6c1e6..046b907268e0d 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -235,7 +235,8 @@ class MPTOptimizer void calcVelocity( std::vector & ref_points, - const std::vector & points) const; + const std::vector & points, + const double yaw_thresh) const; void calcCurvature(std::vector & ref_points) const; @@ -299,6 +300,10 @@ class MPTOptimizer const std::vector & ref_points, std::shared_ptr debug_data_ptr) const; + size_t findNearestIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const double yaw_threshold) const; + public: MPTOptimizer( const bool is_showing_debug_info, const TrajectoryParam & traj_param, diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp index 602c5db22e0ac..e9f9b1be23fb0 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -27,6 +27,7 @@ #include "boost/optional/optional_fwd.hpp" #include +#include #include #include @@ -188,14 +189,19 @@ T clipBackwardPoints( template T clipBackwardPoints( - const T & points, const geometry_msgs::msg::Point pos, const double backward_length, - const double delta_length) + const T & points, const geometry_msgs::msg::Pose pose, const double backward_length, + const double delta_length, const double delta_yaw) { if (points.empty()) { return T{}; } - const size_t target_idx = tier4_autoware_utils::findNearestIndex(points, pos); + const auto target_idx_optional = tier4_autoware_utils::findNearestIndex( + points, pose, std::numeric_limits::max(), delta_yaw); + + const size_t target_idx = target_idx_optional + ? *target_idx_optional + : tier4_autoware_utils::findNearestIndex(points, pose.position); const int begin_idx = std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index c6f637416774e..c8a6840f67d2d 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -286,16 +286,32 @@ std::vector MPTOptimizer::getReferencePoints( // interpolate and crop backward const auto interpolated_points = interpolation_utils::getInterpolatedPoints( smoothed_points, mpt_param_.delta_arc_length_for_mpt_points); - const auto cropped_interpolated_points = points_utils::clipBackwardPoints( - interpolated_points, current_ego_pose_.position, traj_param_.backward_fixing_distance, - mpt_param_.delta_arc_length_for_mpt_points); + const auto interpolated_points_with_yaw = + points_utils::convertToPosesWithYawEstimation(interpolated_points); + const auto cropped_interpolated_points_with_yaw = points_utils::clipBackwardPoints( + interpolated_points_with_yaw, current_ego_pose_, traj_param_.backward_fixing_distance, + mpt_param_.delta_arc_length_for_mpt_points, + traj_param_.delta_yaw_threshold_for_closest_point); + const auto cropped_interpolated_points = + points_utils::convertToPoints(cropped_interpolated_points_with_yaw); return points_utils::convertToReferencePoints(cropped_interpolated_points); } // calc non fixed traj points - const size_t seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(smoothed_points, fixed_ref_points.back().p); + const auto fixed_ref_points_with_yaw = points_utils::convertToPosesWithYawEstimation( + points_utils::convertToPoints(fixed_ref_points)); + const auto seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex( + smoothed_points, fixed_ref_points_with_yaw.back(), std::numeric_limits::max(), + traj_param_.delta_yaw_threshold_for_closest_point); + + if (!seg_idx_optional) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, + "cannot find nearest segment index in getReferencePoints"); + return std::vector{}; + } + const auto seg_idx = *seg_idx_optional; const auto non_fixed_traj_points = std::vector{ smoothed_points.begin() + seg_idx, smoothed_points.end()}; @@ -322,7 +338,7 @@ std::vector MPTOptimizer::getReferencePoints( // set some information to reference points considering fix kinematics trimPoints(ref_points); calcOrientation(ref_points); - calcVelocity(ref_points, smoothed_points); + calcVelocity(ref_points, smoothed_points, traj_param_.delta_yaw_threshold_for_closest_point); calcCurvature(ref_points); calcArcLength(ref_points); calcPlanningFromEgo( @@ -385,8 +401,9 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) */ // assign fix kinematics - const size_t nearest_ref_idx = - tier4_autoware_utils::findNearestIndex(ref_points, current_ego_pose_.position); + const size_t nearest_ref_idx = findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(ref_points), current_ego_pose_, + traj_param_.delta_yaw_threshold_for_closest_point); // calculate cropped_ref_points.at(nearest_ref_idx) with yaw const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose { @@ -422,8 +439,9 @@ std::vector MPTOptimizer::getFixedReferencePoints( } const auto & prev_ref_points = prev_trajs->mpt_ref_points; - const int nearest_prev_ref_idx = - tier4_autoware_utils::findNearestIndex(prev_ref_points, current_ego_pose_.position); + const int nearest_prev_ref_idx = static_cast(findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(prev_ref_points), current_ego_pose_, + traj_param_.delta_yaw_threshold_for_closest_point)); // calculate begin_prev_ref_idx const int begin_prev_ref_idx = [&]() { @@ -1174,6 +1192,19 @@ std::vector MPTOptimizer::get return traj_points; } +size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const double yaw_threshold) const +{ + const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + + const auto nearest_idx_optional = tier4_autoware_utils::findNearestIndex( + points_with_yaw, pose, std::numeric_limits::max(), yaw_threshold); + return nearest_idx_optional + ? *nearest_idx_optional + : tier4_autoware_utils::findNearestIndex(points_with_yaw, pose.position); +} + void MPTOptimizer::calcOrientation(std::vector & ref_points) const { const auto yaw_angles = slerpYawFromReferencePoints(ref_points); @@ -1188,11 +1219,16 @@ void MPTOptimizer::calcOrientation(std::vector & ref_points) con void MPTOptimizer::calcVelocity( std::vector & ref_points, - const std::vector & points) const + const std::vector & points, + const double yaw_thresh) const { + const auto ref_points_with_yaw = + points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)); for (size_t i = 0; i < ref_points.size(); i++) { - ref_points.at(i).v = points[tier4_autoware_utils::findNearestIndex(points, ref_points.at(i).p)] - .longitudinal_velocity_mps; + ref_points.at(i).v = + points[findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(points), ref_points_with_yaw.at(i), yaw_thresh)] + .longitudinal_velocity_mps; } } @@ -1277,8 +1313,11 @@ void MPTOptimizer::calcExtraPoints( if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { const auto & prev_ref_points = prev_trajs->mpt_ref_points; - const size_t prev_idx = - tier4_autoware_utils::findNearestIndex(prev_ref_points, ref_points.at(i).p); + const auto ref_points_with_yaw = + points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)); + const size_t prev_idx = findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(prev_ref_points), ref_points_with_yaw.at(i), + traj_param_.delta_yaw_threshold_for_closest_point); const double dist_to_nearest_prev_ref = tier4_autoware_utils::calcDistance2d(prev_ref_points.at(prev_idx), ref_points.at(i)); if (dist_to_nearest_prev_ref < 1.0 && prev_ref_points.at(prev_idx).near_objects) { @@ -1337,7 +1376,7 @@ void MPTOptimizer::calcBounds( BoundsCandidates filtered_bounds_candidates; for (const auto & bounds_candidate : bounds_candidates) { // Step 1. Bounds is continuous to the previous one, - // and the overlapped signed length is longer than vehice width + // and the overlapped signed length is longer than vehicle width // overlapped_signed_length already considers vehicle width. const double overlapped_signed_length = calcOverlappedBoundsSignedLength(prev_continuous_bounds, bounds_candidate); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 73ab86215c5da..36c1834f9d27e 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1387,16 +1387,22 @@ ObstacleAvoidancePlanner::alignVelocity( for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) { const auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); - const auto & target_pos = fine_traj_points_with_vel[i].pose.position; - const size_t closest_seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pos); + const auto & target_pose = fine_traj_points_with_vel[i].pose; + const auto closest_seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex( + truncated_points, target_pose, std::numeric_limits::max(), + traj_param_.delta_yaw_threshold_for_closest_point); + + const auto closest_seg_idx = + closest_seg_idx_optional + ? *closest_seg_idx_optional + : tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pose.position); // lerp z fine_traj_points_with_vel[i].pose.position.z = - lerpPoseZ(truncated_points, target_pos, closest_seg_idx); + lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx); // lerp vx - const double target_vel = lerpTwistX(truncated_points, target_pos, closest_seg_idx); + const double target_vel = lerpTwistX(truncated_points, target_pose.position, closest_seg_idx); if (i >= zero_vel_fine_traj_idx) { fine_traj_points_with_vel[i].longitudinal_velocity_mps = 0.0; } else if (target_vel < 1e-6) { // NOTE: velocity may be negative due to linear interpolation