change dp_road_graph to two steps

modules/planning/optimizer/dp_poly_path/dp_poly_path_optimizer.cc

@@ -57,9 +57,14 @@ Status DpPolyPathOptimizer::Process(const SpeedData &speed_data,
   CHECK_NOTNULL(decision_data);
   CHECK_NOTNULL(path_data);
   DPRoadGraph dp_road_graph(config_, init_point, speed_data);
-  if (!dp_road_graph.FindPathTunnel(reference_line, decision_data, path_data)) {
+  if (!dp_road_graph.FindPathTunnel(reference_line, path_data)) {
     AERROR << "Failed to find tunnel in road graph";
-    return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph failed");
+    return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph path generation");
+  }
+  if (!dp_road_graph.ComputeObjectdecision(*path_data, speed_data,
+                                           reference_line, decision_data)) {
+    AERROR << "Failed to make decision based on tunnel";
+    return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph decision ");
   }
   return Status::OK();
 }

modules/planning/optimizer/dp_poly_path/dp_road_graph.cc

@@ -51,16 +51,14 @@ DPRoadGraph::DPRoadGraph(const DpPolyPathConfig &config,
     : config_(config), init_point_(init_point), speed_data_(speed_data) {}
 
 bool DPRoadGraph::FindPathTunnel(const ReferenceLine &reference_line,
-                              DecisionData *const decision_data,
-                              PathData *const path_data) {
-  CHECK_NOTNULL(decision_data);
+                                 PathData *const path_data) {
   CHECK_NOTNULL(path_data);
   if (!Init(reference_line)) {
     AERROR << "Fail to init dp road graph!";
     return false;
   }
   std::vector<DPRoadGraphNode> min_cost_path;
-  if (!Generate(reference_line, decision_data, &min_cost_path)) {
+  if (!Generate(reference_line, &min_cost_path)) {
     AERROR << "Fail to generate graph!";
     return false;
   }
@@ -130,20 +128,13 @@ bool DPRoadGraph::FindPathTunnel(const ReferenceLine &reference_line,
   }
   path_data->set_discretized_path(path_points);
 
-  // compute decision data
-  if (compute_object_decision_from_path(*path_data, speed_data_, reference_line,
-                                        decision_data)) {
-    AINFO << "Computing decision_data in dp path success";
-  } else {
-    AINFO << "Computing decision_data in dp path fail";
-  }
-
   return true;
 }
 
 bool DPRoadGraph::Init(const ReferenceLine &reference_line) {
-  if (!reference_line.get_point_in_frenet_frame({init_point_.path_point().x(),
-      init_point_.path_point().y()}, &init_sl_point_)) {
+  if (!reference_line.get_point_in_frenet_frame(
+          {init_point_.path_point().x(), init_point_.path_point().y()},
+          &init_sl_point_)) {
     AERROR << "Fail to map init point from cartesian to sl coordinate!";
     return false;
   }
@@ -171,8 +162,7 @@ bool DPRoadGraph::Init(const ReferenceLine &reference_line) {
 }
 
 bool DPRoadGraph::Generate(const ReferenceLine &reference_line,
-                                 DecisionData *const decision_data,
-                                 std::vector<DPRoadGraphNode> *min_cost_path) {
+                           std::vector<DPRoadGraphNode> *min_cost_path) {
   if (!min_cost_path) {
     AERROR << "the provided min_cost_path is null";
     return false;
@@ -182,13 +172,13 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line,
     AERROR << "Fail to sampling point with path sampler!";
     return false;
   }
-  path_waypoints.insert(path_waypoints.begin(), std::vector<common::SLPoint>{init_sl_point_});
+  path_waypoints.insert(path_waypoints.begin(),
+                        std::vector<common::SLPoint>{init_sl_point_});
 
   const auto &vehicle_config_ =
       common::VehicleConfigHelper::instance()->GetConfig();
   TrajectoryCost trajectory_cost(config_, reference_line,
-                                 vehicle_config_.vehicle_param(), speed_data_,
-                                 *decision_data);
+                                 vehicle_config_.vehicle_param(), speed_data_);
 
   std::vector<std::vector<DPRoadGraphNode>> graph_nodes(path_waypoints.size());
   graph_nodes[0].push_back(DPRoadGraphNode(init_sl_point_, nullptr, 0.0));
@@ -201,7 +191,7 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line,
     for (std::size_t i = 0; i < level_points.size(); ++i) {
       const auto cur_sl_point = level_points[i];
       graph_nodes[level].push_back(DPRoadGraphNode(cur_sl_point, nullptr));
-      auto& cur_node = graph_nodes[level].back();
+      auto &cur_node = graph_nodes[level].back();
       for (std::size_t j = 0; j < prev_dp_nodes.size(); ++j) {
         const auto *prev_dp_node = &prev_dp_nodes[j];
         const auto prev_sl_point = prev_dp_node->sl_point;
@@ -222,7 +212,7 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line,
   for (std::size_t i = 0; i < last_dp_nodes.size(); ++i) {
     const auto &cur_dp_node = last_dp_nodes[i];
     fake_head.UpdateCost(&cur_dp_node, cur_dp_node.min_cost_curve,
-                          cur_dp_node.min_cost);
+                         cur_dp_node.min_cost);
   }
   const auto *min_cost_node = &fake_head;
   while (min_cost_node->min_cost_prev_node) {
@@ -233,9 +223,10 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line,
   return true;
 }
 
-bool DPRoadGraph::compute_object_decision_from_path(
-    const PathData &path_data, const SpeedData &heuristic_speed_data,
-    const ReferenceLine &reference_line, DecisionData *const decision_data) {
+bool DPRoadGraph::ComputeObjectdecision(const PathData &path_data,
+                                        const SpeedData &heuristic_speed_data,
+                                        const ReferenceLine &reference_line,
+                                        DecisionData *const decision_data) {
   CHECK_NOTNULL(decision_data);
 
   // Compute static obstacle decision
@@ -428,11 +419,10 @@ bool DPRoadGraph::SamplePathWaypoints(
     const ReferenceLine &reference_line,
     const common::TrajectoryPoint &init_point,
     std::vector<std::vector<common::SLPoint>> *const points) {
-
   CHECK(points != nullptr);
 
   common::math::Vec2d init_cartesian_point(init_point.path_point().x(),
-                                       init_point.path_point().y());
+                                           init_point.path_point().y());
   common::SLPoint init_sl_point;
   if (!reference_line.get_point_in_frenet_frame(init_cartesian_point,
                                                 &init_sl_point)) {

modules/planning/optimizer/dp_poly_path/dp_road_graph.h

@@ -47,8 +47,13 @@ class DPRoadGraph {
   ~DPRoadGraph() = default;
 
   bool FindPathTunnel(const ReferenceLine &reference_line,
-                   DecisionData *const decision_data,
-                   PathData *const path_data);
+                      PathData *const path_data);
+
+  bool ComputeObjectdecision(const PathData &path_data,
+                             const SpeedData &heuristic_speed_data,
+                             const ReferenceLine &reference_line,
+                             DecisionData *const decision_data);
+
  private:
   /**
    * an private inner struct for the dp algorithm
@@ -57,11 +62,12 @@ class DPRoadGraph {
    public:
     DPRoadGraphNode() = default;
 
-    DPRoadGraphNode(const common::SLPoint point_sl, const DPRoadGraphNode *node_prev)
+    DPRoadGraphNode(const common::SLPoint point_sl,
+                    const DPRoadGraphNode *node_prev)
         : sl_point(point_sl), min_cost_prev_node(node_prev) {}
 
-    DPRoadGraphNode(const common::SLPoint point_sl, const DPRoadGraphNode *node_prev,
-           const double cost)
+    DPRoadGraphNode(const common::SLPoint point_sl,
+                    const DPRoadGraphNode *node_prev, const double cost)
         : sl_point(point_sl), min_cost_prev_node(node_prev), min_cost(cost) {}
 
     void UpdateCost(const DPRoadGraphNode *node_prev,
@@ -82,13 +88,7 @@ class DPRoadGraph {
   bool Init(const ReferenceLine &reference_line);
 
   bool Generate(const ReferenceLine &reference_line,
-                      DecisionData *const decision_data,
-                      std::vector<DPRoadGraphNode> *min_cost_path);
-
-  bool compute_object_decision_from_path(const PathData &path_data,
-                                         const SpeedData &heuristic_speed_data,
-                                         const ReferenceLine &reference_line,
-                                         DecisionData *const decision_data);
+                std::vector<DPRoadGraphNode> *min_cost_path);
 
   bool fill_ego_by_time(
       const FrenetFramePath &frenet_frame_path,
@@ -96,9 +96,10 @@ class DPRoadGraph {
       const SpeedData &heuristic_speed_data, int evaluate_times,
       std::vector<::apollo::common::math::Box2d> *ego_by_time);
 
-  bool SamplePathWaypoints(const ReferenceLine &reference_line,
-                  const common::TrajectoryPoint &init_point,
-                  std::vector<std::vector<common::SLPoint>> *const points);
+  bool SamplePathWaypoints(
+      const ReferenceLine &reference_line,
+      const common::TrajectoryPoint &init_point,
+      std::vector<std::vector<common::SLPoint>> *const points);
 
  private:
   DpPolyPathConfig config_;

modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc

@@ -85,8 +85,7 @@ class DpRoadGraphTest : public PlanningTestBase {
 TEST_F(DpRoadGraphTest, speed_road_graph) {
   DPRoadGraph road_graph(dp_poly_path_config_, init_point_, speed_data_);
   ASSERT_TRUE(reference_line_ != nullptr);
-  bool result =
-      road_graph.FindPathTunnel(*reference_line_, &decision_data_, &path_data_);
+  bool result = road_graph.FindPathTunnel(*reference_line_, &path_data_);
   EXPECT_TRUE(result);
   EXPECT_EQ(648, path_data_.discretized_path().num_of_points());
   EXPECT_EQ(648, path_data_.frenet_frame_path().number_of_points());

modules/planning/optimizer/dp_poly_path/trajectory_cost.cc

@@ -37,8 +37,7 @@ using TrajectoryPoint = ::apollo::common::TrajectoryPoint;
 TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config,
                                const ReferenceLine &reference_line,
                                const common::VehicleParam &vehicle_param,
-                               const SpeedData &heuristic_speed_data,
-                               const DecisionData &decision_data)
+                               const SpeedData &heuristic_speed_data)
     : config_(config),
       reference_line_(&reference_line),
       vehicle_param_(vehicle_param),
@@ -47,28 +46,6 @@ TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config,
       std::min(heuristic_speed_data_.total_time(), FLAGS_prediction_total_time);
   evaluate_times_ = static_cast<uint32_t>(
       std::floor(total_time / config.eval_time_interval()));
-
-  // Mapping Static obstacle
-  for (const auto ptr_static_obstacle : decision_data.StaticObstacles()) {
-    static_obstacle_boxes_.push_back(
-        ptr_static_obstacle->PerceptionBoundingBox());
-  }
-
-  // Mapping dynamic obstacle
-  for (const auto ptr_dynamic_obstacle : decision_data.DynamicObstacles()) {
-    const auto &trajectory = ptr_dynamic_obstacle->Trajectory();
-    std::vector<Box2d> obstacle_by_time;
-    for (std::size_t time = 0; time < evaluate_times_ + 1; ++time) {
-      TrajectoryPoint traj_point = ptr_dynamic_obstacle->GetPointAtTime(
-          time * config.eval_time_interval());
-      obstacle_by_time.push_back(
-          ptr_dynamic_obstacle->GetBoundingBox(traj_point));
-    }
-    dynamic_obstacle_trajectory_.push_back(std::move(obstacle_by_time));
-    dynamic_obstacle_probability_.push_back(trajectory.probability());
-  }
-  CHECK(dynamic_obstacle_trajectory_.size() ==
-        dynamic_obstacle_probability_.size());
 }
 
 double TrajectoryCost::calculate(const QuinticPolynomialCurve1d &curve,

modules/planning/optimizer/dp_poly_path/trajectory_cost.h

@@ -41,8 +41,7 @@ class TrajectoryCost {
   explicit TrajectoryCost(const DpPolyPathConfig &config,
                           const ReferenceLine &reference_line,
                           const common::VehicleParam &vehicle_param,
-                          const SpeedData &heuristic_speed_data,
-                          const DecisionData &decision_data);
+                          const SpeedData &heuristic_speed_data);
   double calculate(const QuinticPolynomialCurve1d &curve, const double start_s,
                    const double end_s) const;
 
@@ -51,9 +50,6 @@ class TrajectoryCost {
   const ReferenceLine *reference_line_ = nullptr;
   const common::VehicleParam vehicle_param_;
   SpeedData heuristic_speed_data_;
-  std::vector<::apollo::common::math::Box2d> static_obstacle_boxes_;
-  std::vector<std::vector<::apollo::common::math::Box2d>> dynamic_obstacle_trajectory_;
-  std::vector<double> dynamic_obstacle_probability_;
   uint32_t evaluate_times_;
 };