planning: add history st graph decision

modules/planning/common/frame.h

@@ -72,6 +72,7 @@ class Frame {
   Obstacle *Find(const std::string &id);
 
   const ReferenceLineInfo *FindDriveReferenceLineInfo();
+
   const ReferenceLineInfo *DriveReferenceLineInfo() const;
 
   const std::vector<const Obstacle *> obstacles() const;
@@ -87,6 +88,10 @@ class Frame {
       const double planning_start_time,
       prediction::PredictionObstacles *prediction_obstacles);
 
+  ADCTrajectory *mutable_trajectory() { return &trajectory_; }
+
+  const ADCTrajectory &trajectory() const { return trajectory_; }
+
  private:
   bool InitReferenceLineInfo();
 
@@ -127,6 +132,8 @@ class Frame {
 
   ChangeLaneDecider change_lane_decider_;
 
+  ADCTrajectory trajectory_;  // last published trajectory
+
   std::unique_ptr<LagPrediction> lag_predictor_;
 };
 

modules/planning/common/path_obstacle.h

@@ -26,8 +26,6 @@
 #include <unordered_map>
 #include <vector>
 
-#include "gtest/gtest_prod.h"
-
 #include "modules/perception/proto/perception_obstacle.pb.h"
 #include "modules/planning/proto/decision.pb.h"
 #include "modules/planning/proto/sl_boundary.pb.h"
@@ -123,19 +121,17 @@ class PathObstacle {
 
   void SetPerceptionSlBoundary(const SLBoundary& sl_boundary);
 
- private:
   /**
-   * @brief check if a ObjectDecisionType is a lateral decision.
+   * @brief check if a ObjectDecisionType is a longitudinal decision.
    **/
-  FRIEND_TEST(IsLateralDecision, AllDecisions);
-  static bool IsLateralDecision(const ObjectDecisionType& decision);
+  static bool IsLongitudinalDecision(const ObjectDecisionType& decision);
 
   /**
-   * @brief check if a ObjectDecisionType is longitudinal decision.
+   * @brief check if a ObjectDecisionType is a lateral decision.
    **/
-  FRIEND_TEST(IsLongitudinalDecision, AllDecisions);
-  static bool IsLongitudinalDecision(const ObjectDecisionType& decision);
+  static bool IsLateralDecision(const ObjectDecisionType& decision);
 
+ private:
   FRIEND_TEST(MergeLongitudinalDecision, AllDecisions);
   static ObjectDecisionType MergeLongitudinalDecision(
       const ObjectDecisionType& lhs, const ObjectDecisionType& rhs);

modules/planning/common/planning_gflags.cc

@@ -178,6 +178,8 @@ DEFINE_bool(enable_nudge_decision, true, "enable nudge decision");
 DEFINE_bool(enable_nudge_slowdown, true,
             "True to slow down when nudge obstacles.");
 
+DEFINE_bool(try_history_decision, false, "try history decision first");
+
 DEFINE_double(static_decision_nudge_l_buffer, 0.3, "l buffer for nudge");
 DEFINE_double(longitudinal_ignore_buffer, 10.0,
               "If an obstacle's longitudinal distance is further away "

modules/planning/common/planning_gflags.h

@@ -101,6 +101,7 @@ DECLARE_double(st_max_t);
 DECLARE_double(static_obstacle_speed_threshold);
 DECLARE_bool(enable_nudge_decision);
 DECLARE_bool(enable_nudge_slowdown);
+DECLARE_bool(try_history_decision);
 DECLARE_double(static_decision_nudge_l_buffer);
 DECLARE_double(longitudinal_ignore_buffer);
 DECLARE_double(lateral_ignore_buffer);

modules/planning/planning.cc

@@ -249,11 +249,15 @@ void Planning::RunOnce() {
   const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
   status = InitFrame(frame_num, stitching_trajectory.back(), start_timestamp,
                      vehicle_state);
-  ADCTrajectory trajectory_pb;
+  if (!frame_) {
+    AERROR << "Failed to init frame";
+    return;
+  }
+  auto* trajectory_pb = frame_->mutable_trajectory();
   if (FLAGS_enable_record_debug) {
-    frame_->RecordInputDebug(trajectory_pb.mutable_debug());
+    frame_->RecordInputDebug(trajectory_pb->mutable_debug());
   }
-  trajectory_pb.mutable_latency_stats()->set_init_frame_time_ms(
+  trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms(
       Clock::NowInSeconds() - start_timestamp);
   if (!status.ok()) {
     AERROR << "Init frame failed";
@@ -263,43 +267,41 @@ void Planning::RunOnce() {
       status.Save(estop.mutable_header()->mutable_status());
       PublishPlanningPb(&estop, start_timestamp);
     }
-    if (frame_) {
-      auto seq_num = frame_->SequenceNum();
-      FrameHistory::instance()->Add(seq_num, std::move(frame_));
-    }
+    auto seq_num = frame_->SequenceNum();
+    FrameHistory::instance()->Add(seq_num, std::move(frame_));
     return;
   }
 
-  status = Plan(start_timestamp, stitching_trajectory, &trajectory_pb);
+  status = Plan(start_timestamp, stitching_trajectory, trajectory_pb);
 
   const auto time_diff_ms = (Clock::NowInSeconds() - start_timestamp) * 1000;
   ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";
 
-  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
-  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();
+  trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
+  ADEBUG << "Planning latency: "
+         << trajectory_pb->latency_stats().DebugString();
 
-  auto* ref_line_task = trajectory_pb.mutable_latency_stats()->add_task_stats();
+  auto* ref_line_task =
+      trajectory_pb->mutable_latency_stats()->add_task_stats();
   ref_line_task->set_time_ms(
       ReferenceLineProvider::instance()->LastTimeDelay() * 1000.0);
   ref_line_task->set_name("ReferenceLineProvider");
 
   if (!status.ok()) {
-    status.Save(trajectory_pb.mutable_header()->mutable_status());
+    status.Save(trajectory_pb->mutable_header()->mutable_status());
     AERROR << "Planning failed:" << status.ToString();
     if (FLAGS_publish_estop) {
       AERROR << "Planning failed and set estop";
-      trajectory_pb.mutable_estop();
+      trajectory_pb->mutable_estop();
     }
   }
 
-  trajectory_pb.set_is_replan(is_replan);
-  PublishPlanningPb(&trajectory_pb, start_timestamp);
-  ADEBUG << "Planning pb:" << trajectory_pb.header().DebugString();
+  trajectory_pb->set_is_replan(is_replan);
+  PublishPlanningPb(trajectory_pb, start_timestamp);
+  ADEBUG << "Planning pb:" << trajectory_pb->header().DebugString();
 
-  if (frame_) {
-    auto seq_num = frame_->SequenceNum();
-    FrameHistory::instance()->Add(seq_num, std::move(frame_));
-  }
+  auto seq_num = frame_->SequenceNum();
+  FrameHistory::instance()->Add(seq_num, std::move(frame_));
 }
 
 void Planning::Stop() {

modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.cc

@@ -38,6 +38,7 @@ namespace planning {
 using apollo::common::ErrorCode;
 using apollo::common::Status;
 using apollo::common::VehicleConfigHelper;
+using apollo::common::TrajectoryPoint;
 using apollo::common::adapter::AdapterManager;
 using apollo::localization::LocalizationEstimate;
 using apollo::planning_internal::STGraphDebug;
@@ -52,38 +53,11 @@ bool DpStSpeedOptimizer::Init(const PlanningConfig& config) {
   return true;
 }
 
-Status DpStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
-                                   const PathData& path_data,
-                                   const common::TrajectoryPoint& init_point,
-                                   const ReferenceLine& reference_line,
-                                   const SpeedData& reference_speed_data,
-                                   PathDecision* const path_decision,
-                                   SpeedData* const speed_data) {
-  if (!is_init_) {
-    AERROR << "Please call Init() before process DpStSpeedOptimizer.";
-    return Status(ErrorCode::PLANNING_ERROR, "Not inited.");
-  }
-
-  if (path_data.discretized_path().NumOfPoints() == 0) {
-    std::string msg("Empty path data");
-    AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
-  }
-
-  StBoundaryMapper boundary_mapper(
-      adc_sl_boundary, st_boundary_config_, reference_line, path_data,
-      dp_st_speed_config_.total_path_length(), dp_st_speed_config_.total_time(),
-      reference_line_info_->IsChangeLanePath());
-
-  // step 1 get boundaries
-  path_decision->EraseStBoundaries();
-  if (boundary_mapper.GetGraphBoundary(path_decision).code() ==
-      ErrorCode::PLANNING_ERROR) {
-    const std::string msg =
-        "Mapping obstacle for dp st speed optimizer failed.";
-    AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
-  }
+bool DpStSpeedOptimizer::SearchStGraph(const StBoundaryMapper& boundary_mapper,
+                                       const PathData& path_data,
+                                       SpeedData* speed_data,
+                                       PathDecision* path_decision,
+                                       STGraphDebug* st_graph_debug) const {
   std::vector<const StBoundary*> boundaries;
   for (const auto* obstacle : path_decision->path_obstacles().Items()) {
     if (!obstacle->st_boundary().IsEmpty()) {
@@ -99,27 +73,100 @@ Status DpStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
     const std::string msg =
         "Getting speed limits for dp st speed optimizer failed!";
     AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
+    return false;
   }
 
   const double path_length = path_data.discretized_path().Length();
-  StGraphData st_graph_data(boundaries, init_point, speed_limit, path_length);
+  StGraphData st_graph_data(boundaries, init_point_, speed_limit, path_length);
 
-  DpStGraph st_graph(reference_line, st_graph_data, dp_st_speed_config_,
-                     path_data, adc_sl_boundary);
-  auto* debug = reference_line_info_->mutable_debug();
-  STGraphDebug* st_graph_debug = debug->mutable_planning_data()->add_st_graph();
+  DpStGraph st_graph(*reference_line_, st_graph_data, dp_st_speed_config_,
+                     path_data, adc_sl_boundary_);
 
   if (!st_graph.Search(path_decision, speed_data).ok()) {
-    const std::string msg(Name() +
-                          ":Failed to search graph with dynamic programming.");
+    const std::string msg(
+        "With history decision: failed to search graph with dynamic "
+        "programming.");
     AERROR << msg;
     RecordSTGraphDebug(st_graph_data, st_graph_debug);
+    return false;
+  }
+  RecordSTGraphDebug(st_graph_data, st_graph_debug);
+  return true;
+}
+
+bool DpStSpeedOptimizer::CreateStBoundaryWithHistoryDecision(
+    const StBoundaryMapper& boundary_mapper, const PathData& path_data,
+    SpeedData* speed_data, PathDecision* path_decision) {
+  if (!FLAGS_try_history_decision) {
+    return false;
+  }
+  path_decision->EraseStBoundaries();
+  const auto* last_frame = FrameHistory::instance()->Latest();
+  if (!last_frame) {
+    return false;
+  }
+  if (boundary_mapper
+          .CreateStBoundaryWithHistory(
+              last_frame->trajectory().decision().object_decision(),
+              path_decision)
+          .code() == ErrorCode::PLANNING_ERROR) {
+    const std::string msg =
+        "With history decision: decision for dp st speed optimizer "
+        "failed.";
+    AERROR << msg;
+    return false;
+  }
+  return SearchStGraph(boundary_mapper, path_data, speed_data, path_decision,
+                       nullptr);
+}
+
+Status DpStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
+                                   const PathData& path_data,
+                                   const TrajectoryPoint& init_point,
+                                   const ReferenceLine& reference_line,
+                                   const SpeedData& reference_speed_data,
+                                   PathDecision* const path_decision,
+                                   SpeedData* const speed_data) {
+  if (!is_init_) {
+    AERROR << "Please call Init() before process DpStSpeedOptimizer.";
+    return Status(ErrorCode::PLANNING_ERROR, "Not inited.");
+  }
+  init_point_ = init_point;
+  adc_sl_boundary_ = adc_sl_boundary;
+  reference_line_ = &reference_line;
+
+  if (path_data.discretized_path().NumOfPoints() == 0) {
+    std::string msg("Empty path data");
+    AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  RecordSTGraphDebug(st_graph_data, st_graph_debug);
+  StBoundaryMapper boundary_mapper(
+      adc_sl_boundary, st_boundary_config_, *reference_line_, path_data,
+      dp_st_speed_config_.total_path_length(), dp_st_speed_config_.total_time(),
+      reference_line_info_->IsChangeLanePath());
+
+  auto* debug = reference_line_info_->mutable_debug();
+  STGraphDebug* st_graph_debug = debug->mutable_planning_data()->add_st_graph();
 
+  if (!CreateStBoundaryWithHistoryDecision(boundary_mapper, path_data,
+                                           speed_data, path_decision)) {
+    path_decision->EraseStBoundaries();
+    if (boundary_mapper.CreateStBoundary(path_decision).code() ==
+        ErrorCode::PLANNING_ERROR) {
+      const std::string msg =
+          "Mapping obstacle for dp st speed optimizer failed.";
+      AERROR << msg;
+      return Status(ErrorCode::PLANNING_ERROR, msg);
+    }
+    if (!SearchStGraph(boundary_mapper, path_data, speed_data, path_decision,
+                       st_graph_debug)) {
+      const std::string msg(
+          Name() + ":Failed to search graph with dynamic programming.");
+      AERROR << msg;
+      return Status(ErrorCode::PLANNING_ERROR, msg);
+    }
+  }
   return Status::OK();
 }
 

modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.h

@@ -24,6 +24,7 @@
 #include <string>
 
 #include "modules/planning/proto/dp_st_speed_config.pb.h"
+#include "modules/planning/proto/planning_internal.pb.h"
 #include "modules/planning/proto/st_boundary_config.pb.h"
 
 #include "modules/planning/tasks/speed_optimizer.h"
@@ -46,6 +47,20 @@ class DpStSpeedOptimizer : public SpeedOptimizer {
                                  const SpeedData& reference_speed_data,
                                  PathDecision* const path_decision,
                                  SpeedData* const speed_data) override;
+
+  bool CreateStBoundaryWithHistoryDecision(
+      const StBoundaryMapper& boundary_mapper, const PathData& path_data,
+      SpeedData* speed_data, PathDecision* path_decision);
+
+  bool SearchStGraph(const StBoundaryMapper& boundary_mapper,
+                     const PathData& path_data, SpeedData* speed_data,
+                     PathDecision* path_decision,
+                     planning_internal::STGraphDebug* debug) const;
+
+ private:
+  common::TrajectoryPoint init_point_;
+  const ReferenceLine* reference_line_ = nullptr;
+  SLBoundary adc_sl_boundary_;
   DpStSpeedConfig dp_st_speed_config_;
   StBoundaryConfig st_boundary_config_;
 };

modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc

@@ -88,7 +88,7 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
   }
   // step 1 get boundaries
   path_decision->EraseStBoundaries();
-  if (boundary_mapper.GetGraphBoundary(path_decision).code() ==
+  if (boundary_mapper.CreateStBoundary(path_decision).code() ==
       ErrorCode::PLANNING_ERROR) {
     return Status(ErrorCode::PLANNING_ERROR,
                   "Mapping obstacle for qp st speed optimizer failed!");

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -85,7 +85,7 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
   }
   // step 1 get boundaries
   path_decision->EraseStBoundaries();
-  if (boundary_mapper.GetGraphBoundary(path_decision).code() ==
+  if (boundary_mapper.CreateStBoundary(path_decision).code() ==
       ErrorCode::PLANNING_ERROR) {
     return Status(ErrorCode::PLANNING_ERROR,
                   "Mapping obstacle for qp st speed optimizer failed!");

modules/planning/tasks/speed_optimizer.cc

@@ -110,8 +110,8 @@ void SpeedOptimizer::RecordDebugInfo(const SpeedData& speed_data) {
 }
 
 void SpeedOptimizer::RecordSTGraphDebug(const StGraphData& st_graph_data,
-                                        STGraphDebug* st_graph_debug) {
-  if (!FLAGS_enable_record_debug) {
+                                        STGraphDebug* st_graph_debug) const {
+  if (!FLAGS_enable_record_debug || !st_graph_debug) {
     ADEBUG << "Skip record debug info";
     return;
   }

modules/planning/tasks/speed_optimizer.h

@@ -51,7 +51,7 @@ class SpeedOptimizer : public Task {
                                 const double init_acc) const;
 
   void RecordSTGraphDebug(const StGraphData& st_graph_data,
-                          planning_internal::STGraphDebug* stGraphDebug);
+                          planning_internal::STGraphDebug* stGraphDebug) const;
 
   void RecordDebugInfo(const SpeedData& speed_data);
 };

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -22,6 +22,7 @@
 
 #include <algorithm>
 #include <limits>
+#include <unordered_map>
 #include <utility>
 
 #include "modules/common/proto/pnc_point.pb.h"
@@ -72,7 +73,7 @@ StBoundaryMapper::StBoundaryMapper(const SLBoundary& adc_sl_boundary,
       planning_time_(planning_time),
       is_change_lane_(is_change_lane) {}
 
-Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
+Status StBoundaryMapper::CreateStBoundary(PathDecision* path_decision) const {
   const auto& path_obstacles = path_decision->path_obstacles();
   if (planning_time_ < 0.0) {
     const std::string msg = "Fail to get params since planning_time_ < 0.";
@@ -94,8 +95,6 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
 
   for (const auto* const_path_obstacle : path_obstacles.Items()) {
     auto* path_obstacle = path_decision->Find(const_path_obstacle->Id());
-    StBoundary boundary;
-    boundary.SetId(path_obstacle->Id());
     if (!path_obstacle->HasLongitudinalDecision()) {
       if (!MapWithoutDecision(path_obstacle).ok()) {
         std::string msg = StrCat("Fail to map obstacle ", path_obstacle->Id(),
@@ -126,7 +125,7 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
       }
     } else if (decision.has_follow() || decision.has_overtake() ||
                decision.has_yield()) {
-      if (!MapWithPredictionTrajectory(path_obstacle).ok()) {
+      if (!MapWithDecision(path_obstacle, decision).ok()) {
         AERROR << "Fail to map obstacle " << path_obstacle->Id()
                << " with decision: " << decision.DebugString();
         return Status(ErrorCode::PLANNING_ERROR,
@@ -138,7 +137,7 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
   }
 
   if (stop_obstacle) {
-    bool success = MapStopDecision(stop_obstacle);
+    bool success = MapStopDecision(stop_obstacle, stop_decision);
     if (!success) {
       std::string msg = "Fail to MapStopDecision.";
       AERROR << msg;
@@ -148,8 +147,105 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
   return Status::OK();
 }
 
-bool StBoundaryMapper::MapStopDecision(PathObstacle* stop_obstacle) const {
-  const auto& stop_decision = stop_obstacle->LongitudinalDecision();
+Status StBoundaryMapper::CreateStBoundaryWithHistory(
+    const ObjectDecisions& history_decisions,
+    PathDecision* path_decision) const {
+  const auto& path_obstacles = path_decision->path_obstacles();
+  if (planning_time_ < 0.0) {
+    const std::string msg = "Fail to get params since planning_time_ < 0.";
+    AERROR << msg;
+    return Status(ErrorCode::PLANNING_ERROR, msg);
+  }
+
+  if (path_data_.discretized_path().NumOfPoints() < 2) {
+    AERROR << "Fail to get params because of too few path points. path points "
+              "size: "
+           << path_data_.discretized_path().NumOfPoints() << ".";
+    return Status(ErrorCode::PLANNING_ERROR,
+                  "Fail to get params because of too few path points");
+  }
+
+  std::unordered_map<std::string, ObjectDecisionType> prev_decision_map;
+  for (const auto& history_decision : history_decisions.decision()) {
+    for (const auto& decision : history_decision.object_decision()) {
+      if (PathObstacle::IsLongitudinalDecision(decision) &&
+          !decision.has_ignore()) {
+        prev_decision_map[history_decision.id()] = decision;
+        break;
+      }
+    }
+  }
+
+  PathObstacle* stop_obstacle = nullptr;
+  ObjectDecisionType stop_decision;
+  double min_stop_s = std::numeric_limits<double>::max();
+
+  for (const auto* const_path_obstacle : path_obstacles.Items()) {
+    auto* path_obstacle = path_decision->Find(const_path_obstacle->Id());
+    auto iter = prev_decision_map.find(path_obstacle->Id());
+    ObjectDecisionType decision;
+    if (iter == prev_decision_map.end()) {
+      decision.mutable_ignore();
+    } else {
+      decision = iter->second;
+    }
+    if (!path_obstacle->HasLongitudinalDecision() && decision.has_ignore()) {
+      if (!MapWithoutDecision(path_obstacle).ok()) {
+        std::string msg = StrCat("Fail to map obstacle ", path_obstacle->Id(),
+                                 " without decision.");
+        AERROR << msg;
+        return Status(ErrorCode::PLANNING_ERROR, msg);
+      }
+      continue;
+    }
+    if (path_obstacle->HasLongitudinalDecision()) {
+      decision = path_obstacle->LongitudinalDecision();
+    }
+    if (decision.has_stop()) {
+      const double stop_s = path_obstacle->PerceptionSLBoundary().start_s() +
+                            decision.stop().distance_s();
+      // this is a rough estimation based on reference line s, so that a large
+      // buffer is used.
+      constexpr double stop_buff = 1.0;
+      if (stop_s + stop_buff < adc_sl_boundary_.end_s()) {
+        AERROR << "Invalid stop decision. not stop at behind of current "
+                  "position. stop_s : "
+               << stop_s << ", and current adc_s is; "
+               << adc_sl_boundary_.end_s();
+        return Status(ErrorCode::PLANNING_ERROR, "invalid decision");
+      }
+      if (stop_s < min_stop_s) {
+        stop_obstacle = path_obstacle;
+        min_stop_s = stop_s;
+        stop_decision = decision;
+      }
+    } else if (decision.has_follow() || decision.has_overtake() ||
+               decision.has_yield()) {
+      if (!MapWithDecision(path_obstacle, decision).ok()) {
+        AERROR << "Fail to map obstacle " << path_obstacle->Id()
+               << " with decision: " << decision.DebugString();
+        return Status(ErrorCode::PLANNING_ERROR,
+                      "Fail to map overtake/yield decision");
+      }
+    } else {
+      AWARN << "No mapping for decision: " << decision.DebugString();
+    }
+  }
+
+  if (stop_obstacle) {
+    bool success = MapStopDecision(stop_obstacle, stop_decision);
+    if (!success) {
+      std::string msg = "Fail to MapStopDecision.";
+      AERROR << msg;
+      return Status(ErrorCode::PLANNING_ERROR, msg);
+    }
+  }
+  return Status::OK();
+}
+
+bool StBoundaryMapper::MapStopDecision(
+    PathObstacle* stop_obstacle,
+    const ObjectDecisionType& stop_decision) const {
   DCHECK(stop_decision.has_stop()) << "Must have stop decision";
 
   if (stop_obstacle->PerceptionSLBoundary().start_s() > planning_distance_) {
@@ -347,13 +443,12 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
   return (lower_points->size() > 1 && upper_points->size() > 1);
 }
 
-Status StBoundaryMapper::MapWithPredictionTrajectory(
-    PathObstacle* path_obstacle) const {
-  const auto& obj_decision = path_obstacle->LongitudinalDecision();
-  DCHECK(obj_decision.has_follow() || obj_decision.has_yield() ||
-         obj_decision.has_overtake())
-      << "obj_decision must be follow or yield or overtake.\n"
-      << obj_decision.DebugString();
+Status StBoundaryMapper::MapWithDecision(
+    PathObstacle* path_obstacle, const ObjectDecisionType& decision) const {
+  DCHECK(decision.has_follow() || decision.has_yield() ||
+         decision.has_overtake())
+      << "decision is " << decision.DebugString()
+      << ", but it must be follow or yield or overtake.";
 
   std::vector<STPoint> lower_points;
   std::vector<STPoint> upper_points;
@@ -364,7 +459,7 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
     return Status::OK();
   }
 
-  if (obj_decision.has_follow() && lower_points.back().t() < planning_time_) {
+  if (decision.has_follow() && lower_points.back().t() < planning_time_) {
     const double diff_s = lower_points.back().s() - lower_points.front().s();
     const double diff_t = lower_points.back().t() - lower_points.front().t();
     double extend_lower_s =
@@ -384,20 +479,20 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
   // get characteristic_length and boundary_type.
   StBoundary::BoundaryType b_type = StBoundary::BoundaryType::UNKNOWN;
   double characteristic_length = 0.0;
-  if (obj_decision.has_follow()) {
-    characteristic_length = std::fabs(obj_decision.follow().distance_s());
+  if (decision.has_follow()) {
+    characteristic_length = std::fabs(decision.follow().distance_s());
     b_type = StBoundary::BoundaryType::FOLLOW;
-  } else if (obj_decision.has_yield()) {
-    characteristic_length = std::fabs(obj_decision.yield().distance_s());
+  } else if (decision.has_yield()) {
+    characteristic_length = std::fabs(decision.yield().distance_s());
     boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
                    .ExpandByS(characteristic_length);
     b_type = StBoundary::BoundaryType::YIELD;
-  } else if (obj_decision.has_overtake()) {
-    characteristic_length = std::fabs(obj_decision.overtake().distance_s());
+  } else if (decision.has_overtake()) {
+    characteristic_length = std::fabs(decision.overtake().distance_s());
     b_type = StBoundary::BoundaryType::OVERTAKE;
   } else {
     DCHECK(false) << "Obj decision should be either yield or overtake: "
-                  << obj_decision.DebugString();
+                  << decision.DebugString();
   }
   boundary.SetBoundaryType(b_type);
   boundary.SetId(path_obstacle->obstacle()->Id());

modules/planning/tasks/st_graph/st_boundary_mapper.h

@@ -47,7 +47,15 @@ class StBoundaryMapper {
 
   virtual ~StBoundaryMapper() = default;
 
-  apollo::common::Status GetGraphBoundary(PathDecision* path_decision) const;
+  apollo::common::Status CreateStBoundary(PathDecision* path_decision) const;
+
+  apollo::common::Status CreateStBoundaryWithHistory(
+      const ObjectDecisions& history_decisions,
+      PathDecision* path_decision) const;
+
+  apollo::common::Status CreateStBoundary(
+      PathObstacle* path_obstacle,
+      const ObjectDecisionType& external_decision) const;
 
   virtual apollo::common::Status GetSpeedLimits(
       const IndexedList<std::string, PathObstacle>& path_obstacles,
@@ -71,10 +79,11 @@ class StBoundaryMapper {
 
   apollo::common::Status MapWithoutDecision(PathObstacle* path_obstacle) const;
 
-  bool MapStopDecision(PathObstacle* stop_obstacle) const;
+  bool MapStopDecision(PathObstacle* stop_obstacle,
+                       const ObjectDecisionType& decision) const;
 
-  apollo::common::Status MapWithPredictionTrajectory(
-      PathObstacle* path_obstacle) const;
+  apollo::common::Status MapWithDecision(
+      PathObstacle* path_obstacle, const ObjectDecisionType& decision) const;
 
   FRIEND_TEST(StBoundaryMapperTest, get_centric_acc_limit);
   double GetCentricAccLimit(const double kappa) const;