Planning: fixed a mapping bug for stop decisions in st_boundary_mapper.

modules/planning/common/path/frenet_frame_path.cc

@@ -23,8 +23,8 @@
 #include <utility>
 
 #include "modules/common/log.h"
-#include "modules/common/proto/pnc_point.pb.h"
 #include "modules/common/math/linear_interpolation.h"
+#include "modules/common/proto/pnc_point.pb.h"
 
 namespace apollo {
 namespace planning {
@@ -38,7 +38,7 @@ void FrenetFramePath::set_points(std::vector<common::FrenetFramePoint> points) {
   points_ = std::move(points);
 }
 
-const std::vector<common::FrenetFramePoint> &FrenetFramePath::points() const {
+const std::vector<common::FrenetFramePoint>& FrenetFramePath::points() const {
   return points_;
 }
 
@@ -49,21 +49,14 @@ double FrenetFramePath::Length() const {
   return points_.back().s() - points_.front().s();
 }
 
-std::uint32_t FrenetFramePath::NumOfPoints() const {
-  return points_.size();
-}
+std::uint32_t FrenetFramePath::NumOfPoints() const { return points_.size(); }
 
-const common::FrenetFramePoint &FrenetFramePath::PointAt(
+const common::FrenetFramePoint& FrenetFramePath::PointAt(
     const std::uint32_t index) const {
   CHECK_LT(index, points_.size());
   return points_[index];
 }
 
-common::FrenetFramePoint &FrenetFramePath::PointAt(const std::uint32_t index) {
-  CHECK_LT(index, points_.size());
-  return points_[index];
-}
-
 common::FrenetFramePoint FrenetFramePath::EvaluateByS(const double s) const {
   CHECK_GT(points_.size(), 1);
   CHECK(s < points_.back().s() + 1.0e-6 && s > points_.front().s() - 1.0e-6);
@@ -90,9 +83,7 @@ common::FrenetFramePoint FrenetFramePath::EvaluateByS(const double s) const {
   return p;
 }
 
-void FrenetFramePath::Clear() {
-  points_.clear();
-}
+void FrenetFramePath::Clear() { points_.clear(); }
 
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/path/frenet_frame_path.h

@@ -39,7 +39,6 @@ class FrenetFramePath {
   std::uint32_t NumOfPoints() const;
   double Length() const;
   const common::FrenetFramePoint &PointAt(const std::uint32_t index) const;
-  common::FrenetFramePoint &PointAt(const std::uint32_t index);
   common::FrenetFramePoint EvaluateByS(const double s) const;
 
   virtual void Clear();

modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc

@@ -424,9 +424,9 @@ void QpFrenetFrame::CalculateHDMapBound() {
     bool suc = reference_line_.get_lane_width(evaluated_knots_[i], &left_bound,
                                               &right_bound);
     if (!suc) {
-      AERROR << "Extracting lane width failed at s = " << evaluated_knots_[i];
-      left_bound = FLAGS_default_reference_line_width / 2;
+      AWARN << "Extracting lane width failed at s = " << evaluated_knots_[i];
       right_bound = FLAGS_default_reference_line_width / 2;
+      left_bound = FLAGS_default_reference_line_width / 2;
     }
 
     hdmap_bound_[i].first = -right_bound + vehicle_param_.width() / 2;
@@ -434,10 +434,10 @@ void QpFrenetFrame::CalculateHDMapBound() {
 
     if (hdmap_bound_[i].first >= hdmap_bound_[i].second) {
       AERROR << "HD Map bound at " << evaluated_knots_[i] << " is infeasible ("
-             << hdmap_bound_[i].first << ", " << hdmap_bound_[i].second
-             << ") " << std::endl;
+             << hdmap_bound_[i].first << ", " << hdmap_bound_[i].second << ") "
+             << std::endl;
       feasible_longitudinal_upper_bound_ =
-        std::min(evaluated_knots_[i], feasible_longitudinal_upper_bound_);
+          std::min(evaluated_knots_[i], feasible_longitudinal_upper_bound_);
     }
   }
 }

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -186,6 +186,10 @@ bool StBoundaryMapper::MapStopDecision(const PathObstacle& stop_obstacle,
   DCHECK(stop_decision.has_stop()) << "Must have stop decision";
 
   PathPoint obstacle_point;
+  if (stop_obstacle.perception_sl_boundary().start_s() >
+      path_data_.frenet_frame_path().points().back().s()) {
+    return true;
+  }
   if (!path_data_.GetPathPointWithRefS(
           stop_obstacle.perception_sl_boundary().start_s(), &obstacle_point)) {
     AERROR << "Fail to get path point from reference s. The sl boundary of "