Planning: disable reanchoring in iterative smoother

modules/common/data/global_flagfile.txt

@@ -12,3 +12,5 @@
 --use_navigation_mode=false
 
 --map_dir=/apollo/modules/map/data/sunnyvale_with_two_offices
+
+--map_dir=/apollo/modules/map/data/san_mateo

modules/planning/conf/planning.conf

@@ -33,4 +33,4 @@
 
 --open_space_planning_period=1000.0
 
---noenable_scenario_pull_over
+--enable_scenario_pull_over

modules/planning/open_space/trajectory_smoother/iterative_anchoring_smoother.cc

@@ -128,14 +128,13 @@ bool IterativeAnchoringSmoother::Smooth(
 
   // Check initial path collision avoidance, if it fails, smoother assumption
   // fails. Try reanchoring
-  std::vector<size_t> colliding_point_index;
   if (!CheckCollisionAvoidance(interpolated_warm_start_path,
-                               &colliding_point_index)) {
+                               &input_colliding_point_index_)) {
     AERROR << "Interpolated input path points colliding with obstacle";
-    if (!ReAnchoring(colliding_point_index, &interpolated_warm_start_path)) {
-      AERROR << "Fail to reanchor colliding input path points";
-      return false;
-    }
+    // if (!ReAnchoring(colliding_point_index, &interpolated_warm_start_path)) {
+    //   AERROR << "Fail to reanchor colliding input path points";
+    //   return false;
+    // }
   }
 
   const auto path_smooth_start_timestamp = std::chrono::system_clock::now();
@@ -491,6 +490,18 @@ bool IterativeAnchoringSmoother::CheckCollisionAvoidance(
   colliding_point_index->clear();
   size_t path_points_size = path_points.size();
   for (size_t i = 0; i < path_points_size; ++i) {
+    // Skip checking collision for thoese points colliding originally
+    bool skip_checking = false;
+    for (const auto index : input_colliding_point_index_) {
+      if (i == index) {
+        skip_checking = true;
+        break;
+      }
+    }
+    if (skip_checking) {
+      continue;
+    }
+
     const double heading = gear_
                                ? path_points[i].theta()
                                : NormalizeAngle(path_points[i].theta() + M_PI);

modules/planning/open_space/trajectory_smoother/iterative_anchoring_smoother.h

@@ -101,6 +101,8 @@ class IterativeAnchoringSmoother {
   std::vector<std::vector<common::math::LineSegment2d>>
       obstacles_linesegments_vec_;
 
+  std::vector<size_t> input_colliding_point_index_;
+
   // gear DRIVE as true and gear REVERSE as false
   bool gear_ = false;
 };