Planning: fixed heading issues in side pass.

modules/planning/common/BUILD

@@ -7,6 +7,9 @@ cc_library(
     hdrs = [
         "indexed_list.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     linkopts = [
         "-lboost_thread",
     ],
@@ -33,6 +36,9 @@ cc_library(
     hdrs = [
         "indexed_queue.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         "//modules/common/util:map_util",
     ],
@@ -66,6 +72,9 @@ cc_library(
     hdrs = [
         "obstacle.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":indexed_list",
         "//modules/common/configs:vehicle_config_helper",
@@ -101,6 +110,9 @@ cc_library(
     hdrs = [
         "planning_context.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":planning_gflags",
         "//cyber/common:log",
@@ -120,6 +132,9 @@ cc_library(
     hdrs = [
         "path_decision.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":obstacle",
         "//modules/planning/reference_line",
@@ -134,6 +149,9 @@ cc_library(
     hdrs = [
         "planning_gflags.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         "//external:gflags",
     ],
@@ -147,6 +165,9 @@ cc_library(
     hdrs = [
         "reference_line_info.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":ego_info",
         ":path_decision",
@@ -186,6 +207,9 @@ cc_library(
     hdrs = [
         "speed_profile_generator.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":frame",
         ":reference_line_info",
@@ -234,6 +258,9 @@ cc_library(
     hdrs = [
         "change_lane_decider.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":planning_gflags",
         ":reference_line_info",
@@ -247,6 +274,9 @@ cc_library(
     hdrs = [
         "local_view.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         "//modules/localization/proto:localization_proto",
         "//modules/map/relative_map/proto:navigation_proto",
@@ -266,6 +296,9 @@ cc_library(
         "frame.h",
         "frame_manager.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":local_view",
         ":change_lane_decider",
@@ -315,6 +348,9 @@ cc_library(
     hdrs = [
         "speed_limit.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         "//modules/common/math",
         "//modules/planning/proto:planning_proto",
@@ -341,6 +377,9 @@ cc_library(
     hdrs = [
         "ego_info.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":obstacle",
         "//cyber/common:log",
@@ -361,6 +400,9 @@ cc_library(
     hdrs = [
         "threshold.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":frame",
         ":speed_limit",
@@ -398,6 +440,9 @@ cc_test(
 
 cc_library(
     name = "planning_common",
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         "threshold",
         ":ego_info",
@@ -416,6 +461,9 @@ cc_library(
     hdrs = [
         "trajectory_info.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         "//modules/planning/common/path:path_data",
         "//modules/planning/common/speed:speed_data",

modules/planning/common/path/BUILD

@@ -10,10 +10,13 @@ cc_library(
     hdrs = [
         "discretized_path.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
-        "//modules/common/proto:pnc_point_proto",
         "//modules/common",
         "//modules/common/math:linear_interpolation",
+        "//modules/common/proto:pnc_point_proto",
         "//modules/planning/common:planning_context",
     ],
 )
@@ -39,6 +42,9 @@ cc_library(
     hdrs = [
         "frenet_frame_path.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         "//modules/common/math:linear_interpolation",
         "//modules/common/proto:pnc_point_proto",
@@ -66,6 +72,9 @@ cc_library(
     hdrs = [
         "path_data.h",
     ],
+    copts = [
+        "-DMODULE_NAME=\\\"planning\\\"",
+    ],
     deps = [
         ":discretized_path",
         ":frenet_frame_path",

modules/planning/common/path/path_data.cc

@@ -177,6 +177,7 @@ bool PathData::SLToXY(const FrenetFramePath &frenet_path,
     double theta = CartesianFrenetConverter::CalculateTheta(
         ref_point.heading(), ref_point.kappa(), frenet_point.l(),
         frenet_point.dl());
+    ADEBUG << "frenet_point: " << frenet_point.ShortDebugString();
     double kappa = CartesianFrenetConverter::CalculateKappa(
         ref_point.kappa(), ref_point.dkappa(), frenet_point.l(),
         frenet_point.dl(), frenet_point.ddl());
@@ -209,15 +210,23 @@ bool PathData::XYToSL(const DiscretizedPath &discretized_path,
   std::vector<common::FrenetFramePoint> frenet_frame_points;
   const double max_len = reference_line_->Length();
   for (const auto &path_point : discretized_path.path_points()) {
-    SLPoint sl_point;
-    if (!reference_line_->XYToSL({path_point.x(), path_point.y()}, &sl_point)) {
-      AERROR << "Fail to transfer cartesian point to frenet point.";
-      return false;
+    common::FrenetFramePoint frenet_point =
+        reference_line_->GetFrenetPoint(path_point);
+    if (!frenet_point.has_s()) {
+      SLPoint sl_point;
+      if (!reference_line_->XYToSL({path_point.x(), path_point.y()},
+                                   &sl_point)) {
+        AERROR << "Fail to transfer cartesian point to frenet point.";
+        return false;
+      }
+      common::FrenetFramePoint frenet_point;
+      // NOTICE: does not set dl and ddl here. Add if needed.
+      frenet_point.set_s(std::max(0.0, std::min(sl_point.s(), max_len)));
+      frenet_point.set_l(sl_point.l());
+      frenet_frame_points.push_back(std::move(frenet_point));
+      continue;
     }
-    common::FrenetFramePoint frenet_point;
-    // NOTICE: does not set dl and ddl here. Add if needed.
-    frenet_point.set_s(std::max(0.0, std::min(sl_point.s(), max_len)));
-    frenet_point.set_l(sl_point.l());
+    frenet_point.set_s(std::max(0.0, std::min(frenet_point.s(), max_len)));
     frenet_frame_points.push_back(std::move(frenet_point));
   }
   *frenet_path = FrenetFramePath(std::move(frenet_frame_points));

modules/planning/reference_line/reference_line.cc

@@ -180,15 +180,19 @@ bool ReferenceLine::Shrink(const double s, double look_backward,
 }
 
 common::FrenetFramePoint ReferenceLine::GetFrenetPoint(
-    const common::TrajectoryPoint& traj_point) const {
+    const common::PathPoint& path_point) const {
+  if (reference_points_.empty()) {
+    return common::FrenetFramePoint();
+  }
+
   common::SLPoint sl_point;
-  XYToSL({traj_point.path_point().x(), traj_point.path_point().y()}, &sl_point);
+  XYToSL({path_point.x(), path_point.y()}, &sl_point);
   common::FrenetFramePoint frenet_frame_point;
   frenet_frame_point.set_s(sl_point.s());
   frenet_frame_point.set_l(sl_point.l());
 
-  const double theta = traj_point.path_point().theta();
-  const double kappa = traj_point.path_point().kappa();
+  const double theta = path_point.theta();
+  const double kappa = path_point.kappa();
   const double l = frenet_frame_point.l();
 
   ReferencePoint ref_point = GetReferencePoint(frenet_frame_point.s());

modules/planning/reference_line/reference_line.h

@@ -81,7 +81,7 @@ class ReferenceLine {
   ReferencePoint GetReferencePoint(const double s) const;
 
   common::FrenetFramePoint GetFrenetPoint(
-      const common::TrajectoryPoint& traj_point) const;
+      const common::PathPoint& path_point) const;
 
   std::vector<ReferencePoint> GetReferencePoints(double start_s,
                                                  double end_s) const;

modules/planning/scenarios/side_pass/side_pass_stage.cc

@@ -78,14 +78,13 @@ Stage::StageStatus SidePassBackup::Process(
     double lane_left_width = 0.0;
     double lane_right_width = 0.0;
     reference_line.GetLaneWidth(obstacle->PerceptionSLBoundary().start_s(),
-                                &lane_left_width,
-                                &lane_right_width);
-    double driving_width = std::max(
-        lane_left_width - obstacle->PerceptionSLBoundary().end_l(),
-        lane_right_width + obstacle->PerceptionSLBoundary().start_l());
+                                &lane_left_width, &lane_right_width);
+    double driving_width =
+        std::max(lane_left_width - obstacle->PerceptionSLBoundary().end_l(),
+                 lane_right_width + obstacle->PerceptionSLBoundary().start_l());
     driving_width =
         std::min(lane_left_width + lane_right_width, driving_width) -
-            FLAGS_static_decision_nudge_l_buffer;
+        FLAGS_static_decision_nudge_l_buffer;
     ADEBUG << "driving_width[" << driving_width << "]";
     if (driving_width > kLBufferThreshold) {
       continue;
@@ -244,7 +243,7 @@ Stage::StageStatus SidePassDetectSafety::Process(
 
   const auto adc_frenet_frame_point_ =
       reference_line_info.reference_line().GetFrenetPoint(
-          frame->PlanningStartPoint());
+          frame->PlanningStartPoint().path_point());
 
   bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
       adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
@@ -305,7 +304,7 @@ Stage::StageStatus SidePassPassObstacle::Process(
 
   const auto adc_frenet_frame_point_ =
       reference_line_info.reference_line().GetFrenetPoint(
-          frame->PlanningStartPoint());
+          frame->PlanningStartPoint().path_point());
 
   bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
       adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());

modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.cc

@@ -56,7 +56,7 @@ Stage::StageStatus SidePassStopOnWaitPoint::Process(
   }
 
   const auto adc_frenet_frame_point_ =
-      reference_line.GetFrenetPoint(frame->PlanningStartPoint());
+      reference_line.GetFrenetPoint(frame->PlanningStartPoint().path_point());
 
   if (!GetContext()->path_data_.LeftTrimWithRefS(adc_frenet_frame_point_.s(),
                                                  adc_frenet_frame_point_.l())) {

modules/planning/std_planning.cc

@@ -301,6 +301,9 @@ void StdPlanning::RunOnce(const LocalView& local_view,
 
   status = Plan(start_timestamp, stitching_trajectory, trajectory_pb);
 
+  for (const auto& p : trajectory_pb->trajectory_point()) {
+    ADEBUG << p.DebugString();
+  }
   const auto time_diff_ms = (Clock::NowInSeconds() - start_timestamp) * 1000;
   ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";
 

modules/planning/toolkits/deciders/side_pass_path_decider.cc

@@ -43,7 +43,7 @@ using apollo::common::util::MakePointENU;
 using apollo::hdmap::HDMapUtil;
 
 constexpr double kRoadBuffer = 0.2;
-constexpr double kObstacleLBuffer = 0.2;
+constexpr double kObstacleLBuffer = 1.0;
 constexpr double kObstacleSBuffer = 0.5;
 constexpr double kSidePassPathLength = 50.0;
 
@@ -76,7 +76,7 @@ Status SidePassPathDecider::Process(
   adc_planning_start_point_ = frame->PlanningStartPoint();
   adc_frenet_frame_point_ =
       reference_line_info->reference_line().GetFrenetPoint(
-          frame->PlanningStartPoint());
+          frame->PlanningStartPoint().path_point());
   InitSolver();
 
   nearest_obstacle_ =
@@ -205,7 +205,9 @@ bool SidePassPathDecider::GeneratePath(
   for (size_t i = 0; i < fem_qp_->x().size(); ++i) {
     common::FrenetFramePoint frenet_frame_point;
     ADEBUG << "FrenetFramePath: s = " << accumulated_s
-           << ", l = " << fem_qp_->x()[i];
+           << ", l = " << fem_qp_->x()[i]
+           << ", dl = " << fem_qp_->x_derivative()[i]
+           << ", ddl = " << fem_qp_->x_second_order_derivative()[i];
     if (accumulated_s >= reference_line_info->reference_line().Length()) {
       break;
     }

modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc

@@ -211,7 +211,8 @@ QpPiecewiseJerkPathOptimizer::GetLateralBounds(
 Status QpPiecewiseJerkPathOptimizer::Process(
     const SpeedData& speed_data, const ReferenceLine& reference_line,
     const common::TrajectoryPoint& init_point, PathData* const path_data) {
-  const auto frenet_point = reference_line.GetFrenetPoint(init_point);
+  const auto frenet_point =
+      reference_line.GetFrenetPoint(init_point.path_point());
   const auto& qp_config = config_.qp_piecewise_jerk_path_config();
   const auto& adc_sl = reference_line_info_->AdcSlBoundary();
   const double qp_delta_s = qp_config.qp_delta_s();

modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc

@@ -92,7 +92,7 @@ bool QpSplinePathGenerator::Generate(
     last_discretized_path_ = &path_data_history.back().first;
   }
 
-  init_frenet_point_ = reference_line_.GetFrenetPoint(init_point);
+  init_frenet_point_ = reference_line_.GetFrenetPoint(init_point.path_point());
 
   if (is_change_lane_path_) {
     ref_l_ = init_frenet_point_.l();

modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc

@@ -69,7 +69,8 @@ bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point,
            << init_point.DebugString();
     return false;
   }
-  init_frenet_frame_point_ = reference_line_.GetFrenetPoint(init_point_);
+  init_frenet_frame_point_ =
+      reference_line_.GetFrenetPoint(init_point_.path_point());
 
   waypoint_sampler_->Init(&reference_line_info_, init_sl_point_,
                           init_frenet_frame_point_);