From de6aeab3c6acd33a0bc5c7f227ff91d56ff6b515 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 6 Jun 2022 18:10:18 +0900 Subject: [PATCH] feat(obstacle_avoidance_planner): not only yaw thresh but also dist thresh for find nearest index (#1037) Signed-off-by: Takayuki Murooka --- .../include/obstacle_avoidance_planner/mpt_optimizer.hpp | 2 +- .../obstacle_avoidance_planner/src/mpt_optimizer.cpp | 9 ++++++--- planning/obstacle_avoidance_planner/src/node.cpp | 9 +++++---- 3 files changed, 12 insertions(+), 8 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 4786083dcfadb..a06844d554c77 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 @@ -296,7 +296,7 @@ class MPTOptimizer size_t findNearestIndexWithSoftYawConstraints( const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double yaw_threshold) const; + const double dist_threshold, const double yaw_threshold) const; public: MPTOptimizer( diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 8d68e5f19f7b6..f176343c0d7ff 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -430,6 +430,7 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) // assign fix kinematics const size_t nearest_ref_idx = findNearestIndexWithSoftYawConstraints( points_utils::convertToPoints(ref_points), current_ego_pose_, + traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); // calculate cropped_ref_points.at(nearest_ref_idx) with yaw @@ -468,6 +469,7 @@ std::vector MPTOptimizer::getFixedReferencePoints( const auto & prev_ref_points = prev_trajs->mpt_ref_points; const int nearest_prev_ref_idx = static_cast(findNearestIndexWithSoftYawConstraints( points_utils::convertToPoints(prev_ref_points), current_ego_pose_, + traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point)); // calculate begin_prev_ref_idx @@ -1222,12 +1224,12 @@ std::vector MPTOptimizer::get size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints( const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double yaw_threshold) const + const double dist_threshold, 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); + const auto nearest_idx_optional = + tier4_autoware_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); return nearest_idx_optional ? *nearest_idx_optional : tier4_autoware_utils::findNearestIndex(points_with_yaw, pose.position); @@ -1330,6 +1332,7 @@ void MPTOptimizer::calcExtraPoints( 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_dist_threshold_for_closest_point, 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)); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index ad5d933952105..060c40ee0ad55 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -203,12 +203,12 @@ std::tuple, std::vector> calcVehicleCirclesInfo( size_t findNearestIndexWithSoftYawConstraints( const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double yaw_threshold) + const double dist_threshold, const double yaw_threshold) { 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); + const auto nearest_idx_optional = + tier4_autoware_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); return nearest_idx_optional ? *nearest_idx_optional : tier4_autoware_utils::findNearestIndex(points_with_yaw, pose.position); @@ -1083,6 +1083,7 @@ void ObstacleAvoidancePlanner::calcVelocity( for (size_t i = 0; i < traj_points.size(); i++) { const size_t nearest_path_idx = findNearestIndexWithSoftYawConstraints( points_utils::convertToPoints(path_points), traj_points.at(i).pose, + traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); const size_t second_nearest_path_idx = [&]() -> size_t { if (nearest_path_idx == 0) { @@ -1464,7 +1465,7 @@ ObstacleAvoidancePlanner::alignVelocity( 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(), + truncated_points, target_pose, traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); const auto closest_seg_idx =