fix(obstacle_avoidance_planner): deal with self-crossing and u-turn path

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

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp

@@ -203,6 +203,9 @@ struct TrajectoryParam
   double acceleration_for_non_deceleration_range;
   int num_fix_points_for_extending;
   double max_dist_for_extending_end_point;
+
+  double ego_nearest_dist_threshold;
+  double ego_nearest_yaw_threshold;
 };
 
 struct MPTParam

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -294,10 +294,6 @@ class MPTOptimizer
     const std::vector<ReferencePoint> & ref_points,
     std::shared_ptr<DebugData> debug_data_ptr) const;
 
-  size_t findNearestIndexWithSoftYawConstraints(
-    const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
-    const double dist_threshold, const double yaw_threshold) const;
-
 public:
   MPTOptimizer(
     const bool is_showing_debug_info, const TrajectoryParam & traj_param,

planning/obstacle_avoidance_planner/src/costmap_generator.cpp

@@ -85,6 +85,7 @@ bool isAvoidingObject(
     return false;
   }
 
+  // TODO(murooka) remove findNearestIndex without any constraints
   const int nearest_idx = motion_utils::findNearestIndex(
     path_points, object.kinematics.initial_pose_with_covariance.pose.position);
   const auto nearest_path_point = path_points[nearest_idx];
@@ -272,6 +273,7 @@ cv::Mat CostmapGenerator::drawObstaclesOnImage(
 
   // fill between objects in the same side
   const auto get_closest_obj_point = [&](size_t idx) {
+    // TODO(murooka) remove findNearestIndex without any constraints
     const auto & path_point =
       path_points.at(motion_utils::findNearestIndex(path_points, obj_positions.at(idx)));
     double min_dist = std::numeric_limits<double>::min();

planning/obstacle_avoidance_planner/src/cv_utils.cpp

@@ -213,6 +213,7 @@ std::vector<cv::Point> getCVPolygon(
   const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
   const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info)
 {
+  // TODO(murooka) remove findNearestIndex without any constraints
   const int nearest_idx = motion_utils::findNearestIndex(
     path_points, object.kinematics.initial_pose_with_covariance.pose.position);
   const auto nearest_path_point = path_points[nearest_idx];
@@ -249,6 +250,7 @@ std::vector<cv::Point> getExtendedCVPolygon(
   }
   const Edges edges = optional_edges.get();
 
+  // TODO(murooka) remove findNearestIndex without any constraints
   const int nearest_polygon_idx = motion_utils::findNearestIndex(points_in_image, edges.origin);
   std::vector<cv::Point> cv_polygon;
   if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) {

planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp

@@ -236,10 +236,9 @@ std::vector<geometry_msgs::msg::Pose> EBPathOptimizer::getFixedPoints(
       std::vector<geometry_msgs::msg::Pose> empty_points;
       return empty_points;
     }
-    const auto opt_begin_idx = motion_utils::findNearestIndex(
-      prev_trajs->smoothed_trajectory, ego_pose, std::numeric_limits<double>::max(),
-      traj_param_.delta_yaw_threshold_for_closest_point);
-    const int begin_idx = opt_begin_idx ? *opt_begin_idx : 0;
+    const size_t begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+      prev_trajs->smoothed_trajectory, ego_pose, traj_param_.ego_nearest_dist_threshold,
+      traj_param_.ego_nearest_yaw_threshold);
     const int backward_fixing_idx = std::max(
       static_cast<int>(
         begin_idx -

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -190,6 +190,18 @@ std::vector<double> slerpYawFromReferencePoints(const std::vector<ReferencePoint
   }
   return interpolation::slerpYawFromPoints(points);
 }
+
+size_t findNearestIndexWithSoftYawConstraints(
+  const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold, const double yaw_threshold)
+{
+  const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points);
+
+  const auto nearest_idx_optional =
+    motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold);
+  return nearest_idx_optional ? *nearest_idx_optional
+                              : motion_utils::findNearestIndex(points_with_yaw, pose.position);
+}
 }  // namespace
 
 MPTOptimizer::MPTOptimizer(
@@ -434,10 +446,10 @@ 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);
+    const size_t nearest_ref_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+      points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)),
+      current_ego_pose_, traj_param_.ego_nearest_dist_threshold,
+      traj_param_.ego_nearest_yaw_threshold);
 
     // calculate cropped_ref_points.at(nearest_ref_idx) with yaw
     const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose {
@@ -473,10 +485,11 @@ 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));
+  const int nearest_prev_ref_idx =
+    static_cast<int>(motion_utils::findFirstNearestIndexWithSoftConstraints(
+      points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(prev_ref_points)),
+      current_ego_pose_, traj_param_.ego_nearest_dist_threshold,
+      traj_param_.ego_nearest_yaw_threshold));
 
   // calculate begin_prev_ref_idx
   const int begin_prev_ref_idx = [&]() {
@@ -723,8 +736,14 @@ boost::optional<Eigen::VectorXd> MPTOptimizer::executeOptimization(
     const size_t D_x = vehicle_model_ptr_->getDimX();
 
     if (prev_trajs && prev_trajs->mpt_ref_points.size() > 1) {
-      const size_t seg_idx =
-        motion_utils::findNearestSegmentIndex(prev_trajs->mpt_ref_points, ref_points.front().p);
+      geometry_msgs::msg::Pose ref_front_point;
+      ref_front_point.position = ref_points.front().p;
+      ref_front_point.orientation =
+        tier4_autoware_utils::createQuaternionFromYaw(ref_points.front().yaw);
+
+      const size_t seg_idx = findNearestIndexWithSoftYawConstraints(
+        points_utils::convertToPoints(prev_trajs->mpt_ref_points), ref_front_point,
+        traj_param_.ego_nearest_dist_threshold, traj_param_.ego_nearest_yaw_threshold);
       double offset = motion_utils::calcLongitudinalOffsetToSegment(
         prev_trajs->mpt_ref_points, seg_idx, ref_points.front().p);
 
@@ -1223,18 +1242,6 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> MPTOptimizer::get
   return traj_points;
 }
 
-size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints(
-  const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
-  const double dist_threshold, const double yaw_threshold) const
-{
-  const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points);
-
-  const auto nearest_idx_optional =
-    motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold);
-  return nearest_idx_optional ? *nearest_idx_optional
-                              : motion_utils::findNearestIndex(points_with_yaw, pose.position);
-}
-
 void MPTOptimizer::calcOrientation(std::vector<ReferencePoint> & ref_points) const
 {
   const auto yaw_angles = slerpYawFromReferencePoints(ref_points);

planning/obstacle_avoidance_planner/src/node.cpp

@@ -38,38 +38,27 @@
 
 namespace
 {
-template <typename T1, typename T2>
-size_t searchExtendedZeroVelocityIndex(
-  const std::vector<T1> & fine_points, const std::vector<T2> & vel_points)
-{
-  const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points);
-  const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1;
-  return motion_utils::findNearestIndex(fine_points, vel_points.at(zero_vel_idx).pose.position);
-}
-
 bool isPathShapeChanged(
   const geometry_msgs::msg::Pose & ego_pose,
   const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
   const std::unique_ptr<std::vector<autoware_auto_planning_msgs::msg::PathPoint>> &
     prev_path_points,
-  const double max_mpt_length, const double max_path_shape_change_dist,
-  const double delta_yaw_threshold)
+  const double max_mpt_length, const double max_path_shape_change_dist, const double dist_threshold,
+  const double yaw_threshold)
 {
   if (!prev_path_points) {
     return false;
   }
 
   // truncate prev points from ego pose to fixed end points
-  const auto opt_prev_begin_idx = motion_utils::findNearestIndex(
-    *prev_path_points, ego_pose, std::numeric_limits<double>::max(), delta_yaw_threshold);
-  const size_t prev_begin_idx = opt_prev_begin_idx ? *opt_prev_begin_idx : 0;
+  const auto prev_begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    *prev_path_points, ego_pose, dist_threshold, yaw_threshold);
   const auto truncated_prev_points =
     points_utils::clipForwardPoints(*prev_path_points, prev_begin_idx, max_mpt_length);
 
   // truncate points from ego pose to fixed end points
-  const auto opt_begin_idx = motion_utils::findNearestIndex(
-    path_points, ego_pose, std::numeric_limits<double>::max(), delta_yaw_threshold);
-  const size_t begin_idx = opt_begin_idx ? *opt_begin_idx : 0;
+  const auto begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    path_points, ego_pose, dist_threshold, yaw_threshold);
   const auto truncated_points =
     points_utils::clipForwardPoints(path_points, begin_idx, max_mpt_length);
 
@@ -200,16 +189,60 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesInfo(
   }
 }
 
-[[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints(
+template <class T>
+size_t findNearestIndexWithSoftYawConstraints(
+  const std::vector<T> & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
+  const double yaw_threshold)
+{
+  const auto nearest_idx_optional =
+    motion_utils::findNearestIndex(points, pose, dist_threshold, yaw_threshold);
+  return nearest_idx_optional ? *nearest_idx_optional
+                              : motion_utils::findNearestIndex(points, pose.position);
+}
+
+template <>
+size_t findNearestIndexWithSoftYawConstraints(
   const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
   const double dist_threshold, const double yaw_threshold)
 {
   const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points);
 
+  return findNearestIndexWithSoftYawConstraints(
+    points_with_yaw, pose, dist_threshold, yaw_threshold);
+}
+
+template <class T>
+size_t findNearestSegmentIndexWithSoftYawConstraints(
+  const std::vector<T> & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
+  const double yaw_threshold)
+{
   const auto nearest_idx_optional =
-    motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold);
+    motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold);
   return nearest_idx_optional ? *nearest_idx_optional
-                              : motion_utils::findNearestIndex(points_with_yaw, pose.position);
+                              : motion_utils::findNearestSegmentIndex(points, pose.position);
+}
+
+template <>
+size_t findNearestSegmentIndexWithSoftYawConstraints(
+  const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold, const double yaw_threshold)
+{
+  const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points);
+
+  return findNearestSegmentIndexWithSoftYawConstraints(
+    points_with_yaw, pose, dist_threshold, yaw_threshold);
+}
+
+template <typename T1, typename T2>
+size_t searchExtendedZeroVelocityIndex(
+  const std::vector<T1> & fine_points, const std::vector<T2> & vel_points,
+  const double yaw_threshold)
+{
+  const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points);
+  const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1;
+  return findNearestIndexWithSoftYawConstraints(
+    fine_points, vel_points.at(zero_vel_idx).pose, std::numeric_limits<double>::max(),
+    yaw_threshold);
 }
 }  // namespace
 
@@ -344,6 +377,11 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
       declare_parameter<bool>("object.avoiding_object_type.pedestrian", true);
     traj_param_.is_avoiding_animal =
       declare_parameter<bool>("object.avoiding_object_type.animal", true);
+
+    // ego nearest search
+    traj_param_.ego_nearest_dist_threshold =
+      declare_parameter<double>("ego_nearest_dist_threshold");
+    traj_param_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
   }
 
   {  // elastic band parameter
@@ -986,8 +1024,8 @@ bool ObstacleAvoidancePlanner::checkReplan(
     traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points;
   if (isPathShapeChanged(
         current_ego_pose_, path_points, prev_path_points_ptr_, max_mpt_length,
-        max_path_shape_change_dist_for_replan_,
-        traj_param_.delta_yaw_threshold_for_closest_point)) {
+        max_path_shape_change_dist_for_replan_, traj_param_.ego_nearest_dist_threshold,
+        traj_param_.ego_nearest_yaw_threshold)) {
     RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path shape was changed.");
     resetPrevOptimization();
     return true;
@@ -1110,15 +1148,9 @@ void ObstacleAvoidancePlanner::calcVelocity(
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points) const
 {
   for (size_t i = 0; i < traj_points.size(); i++) {
-    const size_t nearest_seg_idx = [&]() {
-      const auto opt_seg_idx = motion_utils::findNearestSegmentIndex(
-        path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point,
-        traj_param_.delta_yaw_threshold_for_closest_point);
-      if (opt_seg_idx) {
-        return opt_seg_idx.get();
-      }
-      return motion_utils::findNearestSegmentIndex(path_points, traj_points.at(i).pose.position);
-    }();
+    const size_t nearest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints(
+      path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point,
+      traj_param_.delta_yaw_threshold_for_closest_point);
 
     // add this line not to exceed max index size
     const size_t max_idx = std::min(nearest_seg_idx + 1, path_points.size() - 1);
@@ -1144,8 +1176,9 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea(
   const auto & map_info = cv_maps.map_info;
   const auto & road_clearance_map = cv_maps.clearance_map;
 
-  const size_t nearest_idx =
-    motion_utils::findNearestIndex(traj_points, current_ego_pose_.position);
+  const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    traj_points, current_ego_pose_, traj_param_.ego_nearest_dist_threshold,
+    traj_param_.ego_nearest_yaw_threshold);
 
   // NOTE: Some end trajectory points will be ignored to check if outside the drivable area
   //       since these points might be outside drivable area if only end reference points have high
@@ -1469,8 +1502,9 @@ ObstacleAvoidancePlanner::alignVelocity(
   // search zero velocity index of fine_traj_points
   const size_t zero_vel_fine_traj_idx = [&]() {
     // zero velocity for being outside drivable area
-    const size_t zero_vel_traj_idx =
-      searchExtendedZeroVelocityIndex(fine_traj_points_with_path_zero_vel, traj_points);
+    const size_t zero_vel_traj_idx = searchExtendedZeroVelocityIndex(
+      fine_traj_points_with_path_zero_vel, traj_points,
+      traj_param_.delta_yaw_threshold_for_closest_point);
 
     // zero velocity in path points
     if (opt_zero_vel_path_idx) {
@@ -1492,15 +1526,10 @@ ObstacleAvoidancePlanner::alignVelocity(
     }
 
     const auto & target_pose = fine_traj_points_with_vel[i].pose;
-    const auto closest_seg_idx_optional = motion_utils::findNearestSegmentIndex(
+    const size_t closest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints(
       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 =
-      closest_seg_idx_optional
-        ? *closest_seg_idx_optional
-        : motion_utils::findNearestSegmentIndex(truncated_points, target_pose.position);
-
     // lerp z
     fine_traj_points_with_vel[i].pose.position.z =
       lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx);