Planning: fixed bugs in ParkAndGo scenario

modules/planning/scenarios/util/util.cc

@@ -227,8 +227,8 @@ bool CheckPullOverPositionByDistance(
 ParkAndGoStatus CheckADCParkAndGoCruiseCompleted(
     const ReferenceLineInfo& reference_line_info,
     const ScenarioParkAndGoConfig& scenario_config) {
-  const double kLBuffer = 0.1;
-  const double kHeadingBuffer = 0.05;
+  const double kLBuffer = 0.3;
+  const double kHeadingBuffer = 0.1;
   // check if vehicle in reference line
   const auto& reference_line = reference_line_info.reference_line();
   // get vehicle s,l info
@@ -243,8 +243,12 @@ ParkAndGoStatus CheckADCParkAndGoCruiseCompleted(
   const auto reference_point =
       reference_line.GetReferencePoint(adc_position_sl.s());
   const auto path_point = reference_point.ToPathPoint(adc_position_sl.s());
-  if (std::fabs(adc_position_sl.l() < kLBuffer) &&
+  ADEBUG << "adc_position_sl.l():[" << adc_position_sl.l() << "]";
+  ADEBUG << "adc_heading - path_point.theta():[" << adc_heading << "]"
+         << "[" << path_point.theta() << "]";
+  if (std::fabs(adc_position_sl.l()) < kLBuffer &&
       std::fabs(adc_heading - path_point.theta()) < kHeadingBuffer) {
+    ADEBUG << "cruise completed";
     return CRUISE_COMPLETE;
   }
   return CRUISING;
@@ -259,11 +263,11 @@ bool CheckADCReadyToCruise(Frame* frame,
       common::VehicleStateProvider::Instance()->heading();
   const ReferenceLineInfo& reference_line_info =
       frame->reference_line_info().front();
-  bool no_near_front_obstacle = CheckADCSurroundObstacles(
+  bool is_near_front_obstacle = CheckADCSurroundObstacles(
       adc_position, adc_heading, frame, scenario_config);
   bool heading_align_w_reference_line = CheckADCHeading(
       adc_position, adc_heading, reference_line_info, scenario_config);
-  if (no_near_front_obstacle && heading_align_w_reference_line) {
+  if (!is_near_front_obstacle && heading_align_w_reference_line) {
     return true;
   }
   return false;