added decision category: use the merged longitutional or lateral decision

modules/planning/common/path_obstacle.cc

@@ -131,6 +131,14 @@ ObjectDecisionType PathObstacle::MergeLongitutionalDecision(
                 << " and decision: " << rhs.ShortDebugString();
 }
 
+const ObjectDecisionType& PathObstacle::LongitutionalDecision() const {
+  return longitutional_decision_;
+}
+
+const ObjectDecisionType& PathObstacle::LateralDecision() const {
+  return lateral_decision_;
+}
+
 ObjectDecisionType PathObstacle::MergeLateralDecision(
     const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {
   auto lhs_iter = s_lateral_decision_safety_sorter_.find(lhs.object_tag_case());
@@ -167,6 +175,12 @@ ObjectDecisionType PathObstacle::MergeLateralDecision(
                 << " and decision: " << rhs.ShortDebugString();
 }
 
+bool PathObstacle::HasLateralDecision() const { return has_lateral_decision_; }
+
+bool PathObstacle::HasLongitutionalDecision() const {
+  return has_longitutional_decision_;
+}
+
 const planning::Obstacle* PathObstacle::Obstacle() const { return obstacle_; }
 
 void PathObstacle::AddDecision(const std::string& decider_tag,
@@ -179,10 +193,12 @@ void PathObstacle::AddDecision(const std::string& decider_tag,
           MergeLongitutionalDecision(longitutional_decision_, merged_decision);
     } else {
       lateral_decision_ = merged_decision;
+      has_lateral_decision_ = true;
     }
   } else if (IsLongitutionalDecision(decision)) {
     longitutional_decision_ =
         MergeLongitutionalDecision(longitutional_decision_, decision);
+    has_longitutional_decision_ = true;
   } else {
     DCHECK(false) << "Unkown decision type: " << decision.DebugString();
   }
@@ -190,10 +206,6 @@ void PathObstacle::AddDecision(const std::string& decider_tag,
   decider_tags_.push_back(decider_tag);
 }
 
-const std::vector<ObjectDecisionType>& PathObstacle::Decisions() const {
-  return decisions_;
-}
-
 const std::string PathObstacle::DebugString() const {
   std::stringstream ss;
   for (std::size_t i = 0; i < decisions_.size(); ++i) {

modules/planning/common/path_obstacle.h

@@ -70,8 +70,6 @@ class PathObstacle {
 
   const planning::Obstacle* Obstacle() const;
 
-  bool MergeObstacle() const;
-
   /**
    * @class Add decision to this obstacle. This function will
    * also calculate the merged decision. i.e., if there are multiple lateral
@@ -87,19 +85,19 @@ class PathObstacle {
   void AddDecision(const std::string& decider_tag,
                    const ObjectDecisionType& decision);
 
-  const std::vector<ObjectDecisionType>& Decisions() const;
-
+  bool HasLateralDecision() const;
+  bool HasLongitutionalDecision() const;
   /**
    * return the merged lateral decision
    * Lateral decision is one of {Nudge, Ignore}
    **/
-  const ObjectDecisionType& MergedLateralDecision() const;
+  const ObjectDecisionType& LateralDecision() const;
 
   /**
    * @brief return the merged longitutional decision
    * Longitutional decision is one of {Stop, Yield, Follow, Overtake, Ignore}
    **/
-  const ObjectDecisionType& MergedLongitutionalDecision() const;
+  const ObjectDecisionType& LongitutionalDecision() const;
 
   const std::string DebugString() const;
 
@@ -137,7 +135,9 @@ class PathObstacle {
   // StGraphBoundary st_boundary_;
 
   ObjectDecisionType lateral_decision_;
+  bool has_lateral_decision_ = false;
   ObjectDecisionType longitutional_decision_;
+  bool has_longitutional_decision_ = false;
 
   struct ObjectTagCaseHash {
     std::size_t operator()(

modules/planning/optimizer/dp_st_speed/dp_st_graph.cc

@@ -409,7 +409,7 @@ Status DpStGraph::GetObjectDecision(const StGraphData& st_graph_data,
     }
   }
   for (const auto* path_obstacle : path_decision->path_obstacles().Items()) {
-    if (path_obstacle->Decisions().empty()) {
+    if (!path_obstacle->HasLongitutionalDecision()) {
       ObjectDecisionType ignore_decision;
       ignore_decision.mutable_ignore();
       path_decision->AddDecision("dp_st_graph", path_obstacle->Id(),
@@ -428,8 +428,7 @@ bool DpStGraph::CreateFollowDecision(
   const double follow_distance_s = -FLAGS_follow_min_distance;
   follow->set_distance_s(follow_distance_s);
 
-  const double reference_line_fence_s =
-      boundary.min_s() + follow_distance_s;
+  const double reference_line_fence_s = boundary.min_s() + follow_distance_s;
   common::PathPoint path_point;
   if (!path_data_.GetPathPointWithRefS(reference_line_fence_s, &path_point)) {
     AERROR << "Failed to get path point from reference line s "

modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc

@@ -112,9 +112,8 @@ bool QpFrenetFrame::GetOverallBound(
     return false;
   }
 
-  std::pair<double, double> obs_bound(
-    -std::numeric_limits<double>::infinity(),
-    std::numeric_limits<double>::infinity());
+  std::pair<double, double> obs_bound(-std::numeric_limits<double>::infinity(),
+                                      std::numeric_limits<double>::infinity());
 
   if (!GetStaticObstacleBound(s, &obs_bound)) {
     return false;
@@ -168,75 +167,77 @@ bool QpFrenetFrame::CalculateDiscretizedVehicleLocation() {
 
 bool QpFrenetFrame::MapDynamicObstacleWithDecision(
     const PathObstacle& path_obstacle) {
-  const std::vector<ObjectDecisionType>& decisions = path_obstacle.Decisions();
   const Obstacle* ptr_obstacle = path_obstacle.Obstacle();
+  if (!path_obstacle.HasLateralDecision()) {
+    AERROR << "object has no lateral decision";
+    return false;
+  }
+  const auto& decision = path_obstacle.LateralDecision();
+  if (!decision.has_nudge()) {
+    AWARN << "only support nudge now";
+    return true;
+  }
+  const auto& nudge = decision.nudge();
+  double buffer = std::fabs(nudge.distance_l());
+
+  int nudge_side = (nudge.type() == ObjectNudge::RIGHT_NUDGE) ? 1 : -1;
+
+  for (const SpeedPoint& veh_point : discretized_vehicle_location_) {
+    double time = veh_point.t();
+    common::TrajectoryPoint trajectory_point =
+        ptr_obstacle->GetPointAtTime(time);
+    common::math::Box2d obs_box =
+        ptr_obstacle->GetBoundingBox(trajectory_point);
+    // project obs_box on reference line
+    std::vector<common::math::Vec2d> corners;
+    obs_box.GetAllCorners(&corners);
+    std::vector<common::SLPoint> sl_corners;
+
+    for (uint32_t i = 0; i < corners.size(); ++i) {
+      common::SLPoint cur_point;
+      if (!reference_line_.get_point_in_frenet_frame(
+              {corners[i].x(), corners[i].y()}, &cur_point)) {
+        AERROR << "Fail to map xy point " << corners[i].DebugString() << " to "
+               << cur_point.ShortDebugString();
 
-  for (const auto& decision : decisions) {
-    if (!decision.has_nudge()) {
-      continue;
+        return false;
+      }
+      // shift box base on buffer
+      cur_point.set_l(cur_point.l() - buffer * nudge_side);
+      sl_corners.push_back(std::move(cur_point));
     }
-    const auto& nudge = decision.nudge();
-    double buffer = std::fabs(nudge.distance_l());
-
-    int nudge_side = (nudge.type() == ObjectNudge::RIGHT_NUDGE) ? 1 : -1;
-
-    for (const SpeedPoint& veh_point : discretized_vehicle_location_) {
-      double time = veh_point.t();
-      common::TrajectoryPoint trajectory_point =
-          ptr_obstacle->GetPointAtTime(time);
-      common::math::Box2d obs_box =
-          ptr_obstacle->GetBoundingBox(trajectory_point);
-      // project obs_box on reference line
-      std::vector<common::math::Vec2d> corners;
-      obs_box.GetAllCorners(&corners);
-      std::vector<common::SLPoint> sl_corners;
-
-      for (uint32_t i = 0; i < corners.size(); ++i) {
-        common::SLPoint cur_point;
-        if (!reference_line_.get_point_in_frenet_frame(
-            {corners[i].x(), corners[i].y()}, &cur_point)) {
-          AERROR << "Fail to map xy point " << corners[i].DebugString()
-                 << " to " << cur_point.ShortDebugString();
-
-          return false;
-        }
-        // shift box base on buffer
-        cur_point.set_l(cur_point.l() - buffer * nudge_side);
-        sl_corners.push_back(std::move(cur_point));
+
+    for (uint32_t i = 0; i < sl_corners.size(); ++i) {
+      common::SLPoint sl_first = sl_corners[i % sl_corners.size()];
+      common::SLPoint sl_second = sl_corners[(i + 1) % sl_corners.size()];
+      if (sl_first.s() < sl_second.s()) {
+        std::swap(sl_first, sl_second);
       }
 
-      for (uint32_t i = 0; i < sl_corners.size(); ++i) {
-        common::SLPoint sl_first = sl_corners[i % sl_corners.size()];
-        common::SLPoint sl_second = sl_corners[(i + 1) % sl_corners.size()];
-        if (sl_first.s() < sl_second.s()) {
-          std::swap(sl_first, sl_second);
-        }
-
-        std::pair<double, double> bound = MapLateralConstraint(
-            sl_first, sl_second, nudge_side,
-            veh_point.s() - vehicle_param_.back_edge_to_center(),
-            veh_point.s() + vehicle_param_.front_edge_to_center());
-
-        // update bound map
-        double s_resolution = std::fabs(veh_point.v() * time_resolution_);
-        double updated_start_s =
-            init_frenet_point_.s() + veh_point.s() - s_resolution;
-        double updated_end_s =
-            init_frenet_point_.s() + veh_point.s() + s_resolution;
-        if (updated_end_s > evaluated_knots_.back() ||
-            updated_start_s < evaluated_knots_.front()) {
-          continue;
-        }
-        std::pair<uint32_t, uint32_t> update_index_range =
-            FindInterval(updated_start_s, updated_end_s);
-
-        for (uint32_t j = update_index_range.first;
-             j < update_index_range.second; ++j) {
-          dynamic_obstacle_bound_[j].first =
-              std::max(bound.first, dynamic_obstacle_bound_[j].first);
-          dynamic_obstacle_bound_[j].second =
-              std::min(bound.second, dynamic_obstacle_bound_[j].second);
-        }
+      std::pair<double, double> bound = MapLateralConstraint(
+          sl_first, sl_second, nudge_side,
+          veh_point.s() - vehicle_param_.back_edge_to_center(),
+          veh_point.s() + vehicle_param_.front_edge_to_center());
+
+      // update bound map
+      double s_resolution = std::fabs(veh_point.v() * time_resolution_);
+      double updated_start_s =
+          init_frenet_point_.s() + veh_point.s() - s_resolution;
+      double updated_end_s =
+          init_frenet_point_.s() + veh_point.s() + s_resolution;
+      if (updated_end_s > evaluated_knots_.back() ||
+          updated_start_s < evaluated_knots_.front()) {
+        continue;
+      }
+      std::pair<uint32_t, uint32_t> update_index_range =
+          FindInterval(updated_start_s, updated_end_s);
+
+      for (uint32_t j = update_index_range.first; j < update_index_range.second;
+           ++j) {
+        dynamic_obstacle_bound_[j].first =
+            std::max(bound.first, dynamic_obstacle_bound_[j].first);
+        dynamic_obstacle_bound_[j].second =
+            std::min(bound.second, dynamic_obstacle_bound_[j].second);
       }
     }
   }
@@ -245,42 +246,42 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
 
 bool QpFrenetFrame::MapStaticObstacleWithDecision(
     const PathObstacle& path_obstacle) {
-  const std::vector<ObjectDecisionType>& object_decisions =
-      path_obstacle.Decisions();
   const auto ptr_obstacle = path_obstacle.Obstacle();
-
-  for (const auto& decision : object_decisions) {
-    if (!decision.has_nudge()) {
-      continue;
+  if (!path_obstacle.HasLateralDecision()) {
+    AERROR << "obstacle has no lateral decision";
+    return false;
+  }
+  const auto& decision = path_obstacle.LateralDecision();
+  if (!decision.has_nudge()) {
+    AWARN << "only support nudge decision now";
+    return true;
+  }
+  const auto& nudge = decision.nudge();
+  const double buffer = std::fabs(nudge.distance_l());
+  if (nudge.type() == ObjectNudge::RIGHT_NUDGE) {
+    const auto& box = ptr_obstacle->PerceptionBoundingBox();
+    std::vector<common::math::Vec2d> corners;
+    box.GetAllCorners(&corners);
+    if (!MapPolygon(corners, buffer, -1, &static_obstacle_bound_)) {
+      AERROR << "fail to map polygon with id " << path_obstacle.Id()
+             << " in qp frenet frame";
+      return false;
     }
-    const auto& nudge = decision.nudge();
-    const double buffer = std::fabs(nudge.distance_l());
-    if (nudge.type() == ObjectNudge::RIGHT_NUDGE) {
-      const auto& box = ptr_obstacle->PerceptionBoundingBox();
-      std::vector<common::math::Vec2d> corners;
-      box.GetAllCorners(&corners);
-      if (!MapPolygon(corners, buffer, -1, &static_obstacle_bound_)) {
-        AERROR << "fail to map polygon with id " << path_obstacle.Id()
-               << " in qp frenet frame";
-        return false;
-      }
-    } else {
-      const auto& box = ptr_obstacle->PerceptionBoundingBox();
-      std::vector<common::math::Vec2d> corners;
-      box.GetAllCorners(&corners);
-      if (!MapPolygon(corners, buffer, 1, &static_obstacle_bound_)) {
-        AERROR << "fail to map polygon with id " << path_obstacle.Id()
-               << "in qp frenet frame";
-        return false;
-      }
+  } else {
+    const auto& box = ptr_obstacle->PerceptionBoundingBox();
+    std::vector<common::math::Vec2d> corners;
+    box.GetAllCorners(&corners);
+    if (!MapPolygon(corners, buffer, 1, &static_obstacle_bound_)) {
+      AERROR << "fail to map polygon with id " << path_obstacle.Id()
+             << "in qp frenet frame";
+      return false;
     }
   }
   return true;
 }
 
 bool QpFrenetFrame::MapPolygon(
-    const std::vector<common::math::Vec2d>& corners,
-    const double buffer,
+    const std::vector<common::math::Vec2d>& corners, const double buffer,
     const int nudge_side,
     std::vector<std::pair<double, double>>* const bound_map) {
   std::vector<common::SLPoint> sl_corners;
@@ -434,6 +435,9 @@ void QpFrenetFrame::CalculateHDMapBound() {
 
 bool QpFrenetFrame::CalculateObstacleBound() {
   for (const auto ptr_path_obstacle : path_obstacles_) {
+    if (!ptr_path_obstacle->HasLateralDecision()) {
+      continue;
+    }
     if (ptr_path_obstacle->Obstacle()->IsStatic()) {
       if (!MapStaticObstacleWithDecision(*ptr_path_obstacle)) {
         AERROR << "mapping obstacle with id [" << ptr_path_obstacle->Id()
@@ -467,8 +471,8 @@ bool QpFrenetFrame::GetBound(
   const double s_low = evaluated_knots_[lower_index];
   const double s_high = evaluated_knots_[lower_index + 1];
   double weight = Double::Compare(s_low, s_high, 1e-8) < 0
-                       ? (s - s_low) / (s_high - s_low)
-                       : 0.0;
+                      ? (s - s_low) / (s_high - s_low)
+                      : 0.0;
 
   double low_first = map_bound[lower_index].first;
   double low_second = map_bound[lower_index].second;
@@ -501,7 +505,8 @@ uint32_t QpFrenetFrame::FindIndex(const double s) const {
       std::lower_bound(evaluated_knots_.begin() + 1, evaluated_knots_.end(), s);
   return std::min(
              static_cast<uint32_t>(evaluated_knots_.size() - 1),
-             static_cast<uint32_t>(lower_bound - evaluated_knots_.begin())) - 1;
+             static_cast<uint32_t>(lower_bound - evaluated_knots_.begin())) -
+         1;
 }
 
 }  // namespace planning

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -103,7 +103,7 @@ Status StBoundaryMapper::GetGraphBoundary(
   double min_stop_s = std::numeric_limits<double>::max();
 
   for (const auto* path_obstacle : path_obstacles.Items()) {
-    if (path_obstacle->Decisions().empty()) {
+    if (!path_obstacle->HasLongitutionalDecision()) {
       const auto ret =
           MapObstacleWithoutDecision(*path_obstacle, st_graph_boundaries);
       if (ret.code() == ErrorCode::PLANNING_ERROR) {
@@ -113,52 +113,49 @@ Status StBoundaryMapper::GetGraphBoundary(
         return Status(ErrorCode::PLANNING_ERROR, msg);
       }
     }
-    for (const auto& decision : path_obstacle->Decisions()) {
-      StGraphBoundary boundary(path_obstacle);
-      if (decision.has_follow()) {
-        const auto ret = MapFollowDecision(*path_obstacle, decision, &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");
-        }
-      } else if (decision.has_stop()) {
-        // TODO(all) change start_s() to st_boundary.min_s()
-        const double stop_s =
-            path_obstacle->perception_sl_boundary().start_s() +
-            decision.stop().distance_s();
-        if (stop_s < adc_front_s_) {
-          AERROR << "Invalid stop decision. not stop at ahead of current "
-                    "position. stop_s : "
-                 << stop_s << ", and current adc_s is; " << adc_front_s_;
-          return Status(ErrorCode::PLANNING_ERROR, "invalid decision");
-        }
-        if (!stop_obstacle) {
-          stop_obstacle = path_obstacle;
-          stop_decision = decision;
-          min_stop_s = stop_s;
-        } else if (stop_s < min_stop_s) {
-          stop_obstacle = path_obstacle;
-          min_stop_s = stop_s;
-          stop_decision = decision;
-        }
-      } else if (decision.has_overtake() || decision.has_yield()) {
-        const auto ret = MapObstacleWithPredictionTrajectory(
-            *path_obstacle, decision, st_graph_boundaries);
-        if (!ret.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 {
-        std::string msg = common::util::StrCat("No mapping for decision: ",
-                                               decision.DebugString());
-        return Status(ErrorCode::PLANNING_SKIP, msg);
+    const auto& decision = path_obstacle->LongitutionalDecision();
+    StGraphBoundary boundary(path_obstacle);
+    if (decision.has_follow()) {
+      const auto ret = MapFollowDecision(*path_obstacle, decision, &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");
+      }
+    } else if (decision.has_stop()) {
+      // TODO(all) change start_s() to st_boundary.min_s()
+      const double stop_s = path_obstacle->perception_sl_boundary().start_s() +
+                            decision.stop().distance_s();
+      if (stop_s < adc_front_s_) {
+        AERROR << "Invalid stop decision. not stop at ahead of current "
+                  "position. stop_s : "
+               << stop_s << ", and current adc_s is; " << adc_front_s_;
+        return Status(ErrorCode::PLANNING_ERROR, "invalid decision");
+      }
+      if (!stop_obstacle) {
+        stop_obstacle = path_obstacle;
+        stop_decision = decision;
+        min_stop_s = stop_s;
+      } else if (stop_s < min_stop_s) {
+        stop_obstacle = path_obstacle;
+        min_stop_s = stop_s;
+        stop_decision = decision;
+      }
+    } else if (decision.has_overtake() || decision.has_yield()) {
+      const auto ret = MapObstacleWithPredictionTrajectory(
+          *path_obstacle, decision, st_graph_boundaries);
+      if (!ret.ok()) {
+        AERROR << "Fail to map obstacle " << path_obstacle->Id()
+               << " with decision: " << decision.DebugString();
+        return Status(ErrorCode::PLANNING_ERROR,
+                      "Fail to map overtake/yield decision");
       }
-      boundary.set_id(path_obstacle->Id());
+    } else {
+      std::string msg = common::util::StrCat("No mapping for decision: ",
+                                             decision.DebugString());
+      return Status(ErrorCode::PLANNING_SKIP, msg);
     }
+    boundary.set_id(path_obstacle->Id());
   }
 
   if (stop_obstacle) {

modules/planning/planner/em/decider.cc

@@ -18,8 +18,8 @@
  * @file decider.cpp
  **/
 
-#include "modules/common/log.h"
 #include "modules/common/configs/vehicle_config_helper.h"
+#include "modules/common/log.h"
 #include "modules/common/vehicle_state/vehicle_state.h"
 #include "modules/planning/planner/em/decider.h"
 
@@ -31,8 +31,7 @@ using common::Status;
 using common::VehicleState;
 
 Decider::Decider(DecisionResult* decision_result)
-: decision_(decision_result) {
-}
+    : decision_(decision_result) {}
 
 const DecisionResult& Decider::Decision() const { return *decision_; }
 
@@ -79,45 +78,42 @@ int Decider::MakeMainStopDecision(Frame* frame,
   const auto& path_obstacles = path_decision->path_obstacles();
   for (const auto path_obstacle : path_obstacles.Items()) {
     const auto& obstacle = path_obstacle->Obstacle();
-    for (const auto& object_decision : path_obstacle->Decisions()) {
-      if (not object_decision.has_stop()) {
-        continue;
-      }
-
-      const auto& reference_line = frame->reference_line();
-      apollo::common::PointENU stop_point = object_decision.stop().stop_point();
-      common::SLPoint stop_line_sl;
-      reference_line.get_point_in_frenet_frame({stop_point.x(), stop_point.y()},
-                                               &stop_line_sl);
-
-      double stop_line_s = stop_line_sl.s();
-      if (stop_line_s < 0 || stop_line_s > reference_line.length()) {
-        AERROR << "Ignore object:" << obstacle->Id() << " fence route_s["
-               << stop_line_s << "] not in range[0, " << reference_line.length()
-               << "]";
-        continue;
-      }
-
-      // check stop_line_s vs adc_s
-      common::SLPoint adc_sl;
-      auto& adc_position = VehicleState::instance()->pose().position();
-      reference_line.get_point_in_frenet_frame(
-          {adc_position.x(), adc_position.y()},
-          &adc_sl);
-      const auto &vehicle_param =
-          common::VehicleConfigHelper::instance()->GetConfig().vehicle_param();
-      if (stop_line_s <= adc_sl.s() + vehicle_param.front_edge_to_center()) {
-        AERROR << "object:" << obstacle->Id() << " fence route_s["
-               << stop_line_s << "] behind adc route_s[" << adc_sl.s()
-               << "]";
-        continue;
-      }
-
-      if (stop_line_s < min_stop_line_s) {
-        min_stop_line_s = stop_line_s;
-        stop_obstacle = obstacle;
-        stop_decision = &(object_decision.stop());
-      }
+    const auto& object_decision = path_obstacle->LongitutionalDecision();
+    if (not object_decision.has_stop()) {
+      continue;
+    }
+
+    const auto& reference_line = frame->reference_line();
+    apollo::common::PointENU stop_point = object_decision.stop().stop_point();
+    common::SLPoint stop_line_sl;
+    reference_line.get_point_in_frenet_frame({stop_point.x(), stop_point.y()},
+                                             &stop_line_sl);
+
+    double stop_line_s = stop_line_sl.s();
+    if (stop_line_s < 0 || stop_line_s > reference_line.length()) {
+      AERROR << "Ignore object:" << obstacle->Id() << " fence route_s["
+             << stop_line_s << "] not in range[0, " << reference_line.length()
+             << "]";
+      continue;
+    }
+
+    // check stop_line_s vs adc_s
+    common::SLPoint adc_sl;
+    auto& adc_position = VehicleState::instance()->pose().position();
+    reference_line.get_point_in_frenet_frame(
+        {adc_position.x(), adc_position.y()}, &adc_sl);
+    const auto& vehicle_param =
+        common::VehicleConfigHelper::instance()->GetConfig().vehicle_param();
+    if (stop_line_s <= adc_sl.s() + vehicle_param.front_edge_to_center()) {
+      AERROR << "object:" << obstacle->Id() << " fence route_s[" << stop_line_s
+             << "] behind adc route_s[" << adc_sl.s() << "]";
+      continue;
+    }
+
+    if (stop_line_s < min_stop_line_s) {
+      min_stop_line_s = stop_line_s;
+      stop_obstacle = obstacle;
+      stop_decision = &(object_decision.stop());
     }
   }
 
@@ -150,8 +146,13 @@ int Decider::SetObjectDecisions(PathDecision* const path_decision) {
     const auto& obstacle = path_obstacle->Obstacle();
     object_decision->set_id(obstacle->Id());
     object_decision->set_perception_id(obstacle->PerceptionId());
-    for (const auto& decision : path_obstacle->Decisions()) {
-      object_decision->add_object_decision()->CopyFrom(decision);
+    if (path_obstacle->HasLateralDecision()) {
+      object_decision->add_object_decision()->CopyFrom(
+          path_obstacle->LateralDecision());
+    }
+    if (path_obstacle->HasLongitutionalDecision()) {
+      object_decision->add_object_decision()->CopyFrom(
+          path_obstacle->LongitutionalDecision());
     }
   }
   return 0;