From dbdc5752791b662c656420f808dbf2a8bf9c7d20 Mon Sep 17 00:00:00 2001
From: Zhang Liangliang <zhangliangliang@baidu.com>
Date: Tue, 29 Aug 2017 14:06:01 -0700
Subject: [PATCH] Planning: uniformed the mapping for follow/overtake/yield.

---
 modules/planning/common/speed/st_boundary.cc  | 30 ++++++++-----------
 .../tasks/st_graph/st_boundary_mapper.cc      | 15 ++--------
 2 files changed, 15 insertions(+), 30 deletions(-)

diff --git a/modules/planning/common/speed/st_boundary.cc b/modules/planning/common/speed/st_boundary.cc
index f8d95308af..0cd4a2c3dd 100644
--- a/modules/planning/common/speed/st_boundary.cc
+++ b/modules/planning/common/speed/st_boundary.cc
@@ -194,18 +194,15 @@ StBoundary StBoundary::ExpandByT(const double t) const {
 
   std::vector<std::pair<STPoint, STPoint>> point_pairs;
 
-  const double lower_left_delta_t = lower_points_[1].t() - lower_points_[0].t();
+  const double left_delta_t = lower_points_[1].t() - lower_points_[0].t();
   const double lower_left_delta_s = lower_points_[1].s() - lower_points_[0].s();
-  const double upper_left_delta_t = upper_points_[1].t() - upper_points_[0].t();
   const double upper_left_delta_s = upper_points_[1].s() - upper_points_[0].s();
 
   point_pairs.emplace_back(
-      STPoint(
-          lower_points_[0].y() - t * lower_left_delta_s / lower_left_delta_t,
-          lower_points_[0].x() - t),
-      STPoint(
-          upper_points_[0].y() - t * upper_left_delta_s / upper_left_delta_t,
-          upper_points_.front().x() - t));
+      STPoint(lower_points_[0].y() - t * lower_left_delta_s / left_delta_t,
+              lower_points_[0].x() - t),
+      STPoint(upper_points_[0].y() - t * upper_left_delta_s / left_delta_t,
+              upper_points_.front().x() - t));
 
   const double kMinSEpsilon = 1e-3;
   point_pairs.front().first.set_s(
@@ -219,22 +216,19 @@ StBoundary StBoundary::ExpandByT(const double t) const {
   size_t length = lower_points_.size();
   DCHECK_GE(length, 2);
 
-  const double lower_right_delta_t =
+  const double right_delta_t =
       lower_points_[length - 1].t() - lower_points_[length - 2].t();
   const double lower_right_delta_s =
       lower_points_[length - 1].s() - lower_points_[length - 2].s();
-  const double upper_right_delta_t =
-      upper_points_[length - 1].t() - upper_points_[length - 2].t();
   const double upper_right_delta_s =
       upper_points_[length - 1].s() - upper_points_[length - 2].s();
 
-  point_pairs.emplace_back(
-      STPoint(lower_points_.back().y() +
-                  t * lower_right_delta_s / lower_right_delta_t,
-              lower_points_.back().x() + t),
-      STPoint(upper_points_.back().y() +
-                  t * upper_right_delta_s / upper_right_delta_t,
-              upper_points_.back().x() + t));
+  point_pairs.emplace_back(STPoint(lower_points_.back().y() +
+                                       t * lower_right_delta_s / right_delta_t,
+                                   lower_points_.back().x() + t),
+                           STPoint(upper_points_.back().y() +
+                                       t * upper_right_delta_s / right_delta_t,
+                                   upper_points_.back().x() + t));
   point_pairs.back().second.set_s(
       std::fmax(point_pairs.back().second.s(),
                 point_pairs.back().first.s() + kMinSEpsilon));
diff --git a/modules/planning/tasks/st_graph/st_boundary_mapper.cc b/modules/planning/tasks/st_graph/st_boundary_mapper.cc
index f19ad3e987..f110bd7a97 100644
--- a/modules/planning/tasks/st_graph/st_boundary_mapper.cc
+++ b/modules/planning/tasks/st_graph/st_boundary_mapper.cc
@@ -117,17 +117,7 @@ Status StBoundaryMapper::GetGraphBoundary(
       continue;
     }
     const auto& decision = path_obstacle->LongitudinalDecision();
-    if (decision.has_follow()) {
-      StBoundary follow_boundary;
-      const auto ret =
-          MapFollowDecision(*path_obstacle, decision, &follow_boundary);
-      if (!ret.ok()) {
-        AERROR << "Fail to map obstacle " << path_obstacle->Id()
-               << " with follow decision: " << decision.DebugString();
-        return Status(ErrorCode::PLANNING_ERROR, "Fail to map follow decision");
-      }
-      AppendBoundary(follow_boundary, st_boundaries);
-    } else if (decision.has_stop()) {
+    if (decision.has_stop()) {
       const double stop_s = path_obstacle->perception_sl_boundary().start_s() +
                             decision.stop().distance_s();
       if (stop_s < adc_sl_boundary_.end_s()) {
@@ -146,7 +136,8 @@ Status StBoundaryMapper::GetGraphBoundary(
         min_stop_s = stop_s;
         stop_decision = decision;
       }
-    } else if (decision.has_overtake() || decision.has_yield()) {
+    } else if (decision.has_follow() || decision.has_overtake() ||
+               decision.has_yield()) {
       StBoundary boundary;
       const auto ret =
           MapWithPredictionTrajectory(*path_obstacle, decision, &boundary);
-- 
GitLab