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,8 +112,7 @@ bool QpFrenetFrame::GetOverallBound(
     return false;
   }
 
-  std::pair<double, double> obs_bound(
-    -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)) {
@@ -168,12 +167,15 @@ 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();
-
-  for (const auto& decision : decisions) {
+  if (!path_obstacle.HasLateralDecision()) {
+    AERROR << "object has no lateral decision";
+    return false;
+  }
+  const auto& decision = path_obstacle.LateralDecision();
   if (!decision.has_nudge()) {
-      continue;
+    AWARN << "only support nudge now";
+    return true;
   }
   const auto& nudge = decision.nudge();
   double buffer = std::fabs(nudge.distance_l());
@@ -195,8 +197,8 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
       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();
+        AERROR << "Fail to map xy point " << corners[i].DebugString() << " to "
+               << cur_point.ShortDebugString();
 
         return false;
       }
@@ -230,8 +232,8 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
       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) {
+      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 =
@@ -239,19 +241,20 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
       }
     }
   }
-  }
   return true;
 }
 
 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 (!path_obstacle.HasLateralDecision()) {
+    AERROR << "obstacle has no lateral decision";
+    return false;
+  }
+  const auto& decision = path_obstacle.LateralDecision();
   if (!decision.has_nudge()) {
-      continue;
+    AWARN << "only support nudge decision now";
+    return true;
   }
   const auto& nudge = decision.nudge();
   const double buffer = std::fabs(nudge.distance_l());
@@ -274,13 +277,11 @@ bool QpFrenetFrame::MapStaticObstacleWithDecision(
       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()
@@ -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,20 +113,18 @@ Status StBoundaryMapper::GetGraphBoundary(
         return Status(ErrorCode::PLANNING_ERROR, msg);
       }
     }
-    for (const auto& decision : path_obstacle->Decisions()) {
+    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");
+        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() +
+      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 "
@@ -159,7 +157,6 @@ Status StBoundaryMapper::GetGraphBoundary(
     }
     boundary.set_id(path_obstacle->Id());
   }
-  }
 
   if (stop_obstacle) {
     StGraphBoundary stop_boundary(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,7 +78,7 @@ 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()) {
+    const auto& object_decision = path_obstacle->LongitutionalDecision();
     if (not object_decision.has_stop()) {
       continue;
     }
@@ -102,14 +101,12 @@ int Decider::MakeMainStopDecision(Frame* frame,
     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 =
+        {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()
-               << "]";
+      AERROR << "object:" << obstacle->Id() << " fence route_s[" << stop_line_s
+             << "] behind adc route_s[" << adc_sl.s() << "]";
       continue;
     }
 
@@ -119,7 +116,6 @@ int Decider::MakeMainStopDecision(Frame* frame,
       stop_decision = &(object_decision.stop());
     }
   }
-  }
 
   if (stop_obstacle != nullptr) {
     MainStop* main_stop = decision_->mutable_main_decision()->mutable_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;