feat(obstacle_avoidance_planner): sanitize reference points (#6704)

* feat(obstacle_avoidance_planner): sanitize reference points Signed-off-by: Daniel Sanchez <[email protected]> * bugfix, first point was excluded Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
autowarefoundation · Apr 1, 2024 · 2d105d5 · 2d105d5
1 parent dadbb2c
commit 2d105d5
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 0 deletions.
diff --git a/.../obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/.../obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp
@@ -129,6 +129,8 @@ ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point);
 std::vector<ReferencePoint> convertToReferencePoints(
   const std::vector<TrajectoryPoint> & traj_points);
 
+std::vector<ReferencePoint> sanitizePoints(const std::vector<ReferencePoint> & points);
+
 void compensateLastPose(
   const PathPoint & last_path_point, std::vector<TrajectoryPoint> & traj_points,
   const double delta_dist_threshold, const double delta_yaw_threshold);

diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
@@ -567,6 +567,9 @@ std::vector<ReferencePoint> MPTOptimizer::calcReferencePoints(
   ref_points = motion_utils::cropPoints(
     ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin,
     backward_traj_length + tmp_margin);
+
+  // remove repeated points
+  ref_points = trajectory_utils::sanitizePoints(ref_points);
   SplineInterpolationPoints2d ref_points_spline(ref_points);
   ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_);
 
@@ -586,6 +589,7 @@ std::vector<ReferencePoint> MPTOptimizer::calcReferencePoints(
   // NOTE: This must be after backward cropping.
   //       New start point may be added and resampled. Spline calculation is required.
   updateFixedPoint(ref_points);
+  ref_points = trajectory_utils::sanitizePoints(ref_points);
   ref_points_spline = SplineInterpolationPoints2d(ref_points);
 
   // 6. update bounds

diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp
@@ -79,6 +79,24 @@ std::vector<ReferencePoint> convertToReferencePoints(
   return ref_points;
 }
 
+std::vector<ReferencePoint> sanitizePoints(const std::vector<ReferencePoint> & points)
+{
+  std::vector<ReferencePoint> output;
+  for (size_t i = 0; i < points.size(); i++) {
+    if (i > 0) {
+      const auto & current_pos = points.at(i).pose.position;
+      const auto & prev_pos = points.at(i - 1).pose.position;
+      if (
+        std::fabs(current_pos.x - prev_pos.x) < 1e-6 &&
+        std::fabs(current_pos.y - prev_pos.y) < 1e-6) {
+        continue;
+      }
+    }
+    output.push_back(points.at(i));
+  }
+  return output;
+}
+
 void compensateLastPose(
   const PathPoint & last_path_point, std::vector<TrajectoryPoint> & traj_points,
   const double delta_dist_threshold, const double delta_yaw_threshold)