feat(obstacle_avoidance_planner): not only yaw thresh but also dist t…

…hresh for find nearest index (#1037)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -296,7 +296,7 @@ class MPTOptimizer
 
   size_t findNearestIndexWithSoftYawConstraints(
     const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
-    const double yaw_threshold) const;
+    const double dist_threshold, const double yaw_threshold) const;
 
 public:
   MPTOptimizer(

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -430,6 +430,7 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector<ReferencePoint> & 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<ReferencePoint> MPTOptimizer::getFixedReferencePoints(
   const auto & prev_ref_points = prev_trajs->mpt_ref_points;
   const int nearest_prev_ref_idx = static_cast<int>(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<autoware_auto_planning_msgs::msg::TrajectoryPoint> MPTOptimizer::get
 
 size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints(
   const std::vector<geometry_msgs::msg::Point> & 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<double>::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));

planning/obstacle_avoidance_planner/src/node.cpp

@@ -203,12 +203,12 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesInfo(
 
 size_t findNearestIndexWithSoftYawConstraints(
   const std::vector<geometry_msgs::msg::Point> & 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<double>::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<double>::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 =