update nudge decision; also add a gflag

modules/planning/common/planning_gflags.cc

@@ -103,6 +103,7 @@ DEFINE_double(stgraph_max_deceleration_divide_factor_level_2, 2.0,
               "The divide factor for max deceleration at level 2.");
 
 // Decision Part
+DEFINE_bool(enable_nudge_decision, true, "enable nudge decision");
 DEFINE_double(static_decision_ignore_s_range, 3.0,
               "threshold for judging nudge in dp path computing decision");
 DEFINE_double(static_decision_nudge_l_buffer, 0.5, "l buffer for nudge");
@@ -118,6 +119,8 @@ DEFINE_double(stop_distance_obstacle, 5.0,
               "stop distance from in-lane obstacle (meters)");
 DEFINE_double(destination_adjust_distance_buffer, 1.0,
               "distance buffer when adjusting destination stop line");
+DEFINE_double(min_driving_width, 2.5,
+              "minimum road width(meters) for adc to drive through");
 DEFINE_double(nudge_distance_vehicle, 0.3,
               "minimum distance to nudge a vehicle");
 DEFINE_double(nudge_distance_bicycle, 0.9144,

modules/planning/common/planning_gflags.h

@@ -74,6 +74,7 @@ DECLARE_double(stgraph_max_deceleration_divide_factor_level_1);
 DECLARE_double(stgraph_max_deceleration_divide_factor_level_2);
 
 // Decision Part
+DECLARE_bool(enable_nudge_decision);
 DECLARE_double(static_decision_ignore_s_range);
 DECLARE_double(static_decision_nudge_l_buffer);
 DECLARE_double(static_decision_stop_buffer);
@@ -81,6 +82,7 @@ DECLARE_double(dynamic_decision_follow_range);
 DECLARE_double(stop_distance_pedestrian);
 DECLARE_double(stop_distance_obstacle);
 DECLARE_double(destination_adjust_distance_buffer);
+DECLARE_double(min_driving_width);
 DECLARE_double(nudge_distance_vehicle);
 DECLARE_double(nudge_distance_bicycle);
 DECLARE_double(nudge_distance_obstacle);

modules/planning/tasks/path_decider/path_decider.cc

@@ -71,6 +71,10 @@ bool PathDecider::MakeObjectDecision(const PathData &path_data,
 
 bool PathDecider::MakeStaticObstacleDecision(
     const PathData &path_data, PathDecision *const path_decision) {
+  if (!FLAGS_enable_nudge_decision) {
+    return true;
+  }
+
   DCHECK_NOTNULL(path_decision);
   std::vector<common::SLPoint> adc_sl_points;
   std::vector<common::math::Box2d> adc_bounding_box;
@@ -137,16 +141,42 @@ bool PathDecider::MakeStaticObstacleDecision(
       }
 
       // check STOP/NUDGE
-      double left_bound;
-      double right_bound;
-      if (!reference_line_->get_lane_width(adc_sl.s(), &left_bound,
-                                           &right_bound)) {
-        left_bound = right_bound = FLAGS_default_reference_line_width / 2;
+      double left_width;
+      double right_width;
+      if (!reference_line_->get_lane_width(adc_sl.s(), &left_width,
+                                           &right_width)) {
+        left_width = right_width = FLAGS_default_reference_line_width / 2;
+      }
+      double driving_width;
+      bool left_nudgable = false;
+      bool right_nudgable = false;
+      if (sl_boundary.start_l() >= 0) {
+        // obstacle on the left, check RIGHT_NUDGE
+        driving_width = sl_boundary.start_l() -
+            FLAGS_static_decision_nudge_l_buffer + right_width;
+        right_nudgable = (driving_width >= FLAGS_min_driving_width) ?
+            true : false;
+      } else if (sl_boundary.end_l() <= 0) {
+        // obstacle on the right, check LEFT_NUDGE
+        driving_width = std::fabs(sl_boundary.end_l()) -
+            FLAGS_static_decision_nudge_l_buffer + left_width;
+        left_nudgable = (driving_width >= FLAGS_min_driving_width) ?
+            true : false;
+      } else {
+        // obstacle across the central line, decide RIGHT_NUDGE/LEFT_NUDGE
+        double driving_width_left = left_width - sl_boundary.end_l() -
+            FLAGS_static_decision_nudge_l_buffer;
+        double driving_width_right = right_width -
+            std::fabs(sl_boundary.start_l()) -
+            FLAGS_static_decision_nudge_l_buffer;
+        if (std::max(driving_width_right, driving_width_left) >=
+            FLAGS_min_driving_width) {
+          // nudgable
+          left_nudgable = driving_width_left > driving_width_right ?
+              true : false;
+          right_nudgable = !left_nudgable;
+        }
       }
-      bool left_nudgable = left_bound - sl_boundary.end_l() >=
-                           FLAGS_static_decision_nudge_l_buffer;
-      bool right_nudgable = sl_boundary.start_l() - right_bound >=
-                            FLAGS_static_decision_nudge_l_buffer;
 
       if (!left_nudgable && !right_nudgable) {
         // STOP: and break