Planning: fixed a bug on get_pathpoint_from_ref_s. (#587)

modules/planning/common/path/BUILD

@@ -43,6 +43,7 @@ cc_library(
         ":frenet_frame_path",
         "//modules/planning/common:planning_gflags",
         "//modules/planning/math:double",
+        "//modules/planning/reference_line",
     ],
 )
 

modules/planning/common/path/path_data.cc

@@ -31,6 +31,9 @@
 namespace apollo {
 namespace planning {
 
+using SLPoint = apollo::common::SLPoint;
+using Vec2d = apollo::common::math::Vec2d;
+
 void PathData::set_discretized_path(const DiscretizedPath &path) {
   discretized_path_ = path;
 }
@@ -59,33 +62,19 @@ bool PathData::get_path_point_with_path_s(
 }
 
 bool PathData::get_path_point_with_ref_s(
-    const double ref_s, common::PathPoint *const path_point) const {
-  const auto &frenet_points = frenet_path_.points();
-  if (frenet_points.size() < 2 || ref_s < frenet_points.front().s() ||
-      frenet_points.back().s() < ref_s) {
-    return false;
-  }
-
-  auto comp = [](const common::FrenetFramePoint &frenet_point,
-                 const double ref_s) { return frenet_point.s() < ref_s; };
-
-  auto it_lower =
-      std::lower_bound(frenet_points.begin(), frenet_points.end(), ref_s, comp);
-  if (it_lower == frenet_points.begin()) {
-    *path_point = discretized_path_.points().front();
-  } else {
-    //        std::uint32_t index_lower = (std::uint32_t)(it_lower -
-    //        frenet_points.begin());
-    //
-    //        double ref_s0 = (it_lower - 1)->s();
-    //        double ref_s1 = it_lower->s();
-    //
-    //        CHECK_LT(ref_s0, ref_s1);
-    //        double weight = (ref_s - ref_s0) / (ref_s1 - ref_s0);
-    //        *path_point =
-    //        common::PathPoint::interpolate_linear_approximation(path_.path_point_at(index_lower
-    //        - 1),
-    //                path_.path_point_at(index_lower), weight);
+    const ReferenceLine &reference_line, const double ref_s,
+    common::PathPoint *const path_point) const {
+  *path_point = discretized_path_.start_point();
+  for (common::PathPoint point : discretized_path_.points()) {
+    SLPoint sl;
+    reference_line.get_point_in_frenet_frame(Vec2d(point.x(), point.y()), &sl);
+    if (Double::Compare(sl.s(), ref_s) == 0) {
+      path_point->CopyFrom(point);
+      return true;
+    }
+    if (std::fabs(sl.s() - ref_s) < std::fabs(path_point->s() - ref_s)) {
+      path_point->CopyFrom(point);
+    }
   }
   return true;
 }

modules/planning/common/path/path_data.h

@@ -25,6 +25,7 @@
 
 #include "modules/planning/common/path/discretized_path.h"
 #include "modules/planning/common/path/frenet_frame_path.h"
+#include "modules/planning/reference_line/reference_line.h"
 
 namespace apollo {
 namespace planning {
@@ -46,7 +47,8 @@ class PathData {
   bool get_path_point_with_path_s(const double s,
                                   common::PathPoint *const path_point) const;
 
-  bool get_path_point_with_ref_s(const double ref_s,
+  bool get_path_point_with_ref_s(const ReferenceLine &reference_line,
+                                 const double ref_s,
                                  common::PathPoint *const path_point) const;
 
   void Clear();

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -303,8 +303,7 @@ Status QpSplineStGraph::GetSConstraintByTime(
     const std::vector<StGraphBoundary>& boundaries, const double time,
     const double total_path_s, double* const s_upper_bound,
     double* const s_lower_bound) const {
-  *s_upper_bound =
-      std::min(total_path_s, time * qp_spline_st_speed_config_.max_speed());
+  *s_upper_bound = total_path_s;
 
   for (const StGraphBoundary& boundary : boundaries) {
     double s_upper = 0.0;

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -172,7 +172,8 @@ bool StBoundaryMapper::MapObstacleWithStopDecision(
 
   PathPoint path_point;
   if (!path_data_.get_path_point_with_ref_s(
-          stop_obstacle.sl_boundary().start_s(), &path_point)) {
+          reference_line_, stop_obstacle.sl_boundary().start_s(),
+          &path_point)) {
     AERROR << "Fail to get path point from reference s. The sl boundary of "
               "stop obstacle is: "
            << stop_obstacle.sl_boundary().DebugString();

modules/planning/optimizer/st_graph/st_graph_boundary.cc

@@ -113,6 +113,7 @@ bool StGraphBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
                                                Vec2d(curr_time, s_high_limit_)};
   *s_upper = s_high_limit_;
   *s_lower = 0.0;
+
   Vec2d p_s_first;
   Vec2d p_s_second;
 
@@ -123,8 +124,7 @@ bool StGraphBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
   }
   if (boundary_type_ == BoundaryType::STOP ||
       boundary_type_ == BoundaryType::YIELD ||
-      boundary_type_ == BoundaryType::FOLLOW ||
-      boundary_type_ == BoundaryType::UNKNOWN) {
+      boundary_type_ == BoundaryType::FOLLOW) {
     *s_upper = std::fmin(*s_upper, std::fmin(p_s_first.y(), p_s_second.y()));
   } else if (boundary_type_ == BoundaryType::OVERTAKE) {
     // overtake