added dp path optimizer (#95)

modules/planning/optimizer/dp_poly_path/BUILD

+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "dp_poly_path",
+    srcs = [
+        "dp_poly_path_config.cpp",
+        "dp_road_graph.cpp",
+        "graph_edge.cpp",
+        "graph_vertex.cpp",
+        "path_sampler.cpp",
+        "trajectory_cost.cpp",
+    ],
+    hdrs = [
+        "dp_poly_path_config.h",
+        "dp_road_graph.h",
+        "graph_edge.h",
+        "graph_vertex.h",
+        "path_sampler.h",
+        "trajectory_cost.h",
+    ],
+    deps = [
+        "@eigen//:eigen",
+        "//modules/common/math",
+        "//modules/map/proto:map_proto",
+        "//modules/planning/common:data_center",
+        "//modules/planning/common:decision_data",
+        "//modules/planning/common:obstacle",
+        "//modules/planning/common/path",
+        "//modules/planning/common/path:discretized_path",
+        "//modules/planning/common/path:frenet_frame_path",
+        "//modules/planning/common/path:path_data",
+        "//modules/planning/common/speed",
+        "//modules/planning/math:sl_analytic_transformation",
+        "//modules/planning/math/curve1d:polynomial_curve1d",
+        "//modules/planning/math/curve1d:quintic_polynomial_curve1d",
+        "//modules/planning/reference_line:lib_reference_line",
+    ],
+)

modules/planning/optimizer/dp_poly_path/dp_poly_path_config.cpp

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file dp_poly_path_config.cpp
+ **/
+
+#include "modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h"
+
+namespace apollo {
+namespace planning {
+
+DpPolyPathConfig::DpPolyPathConfig(const boost::property_tree::ptree &_ptree) {
+  // TODO:need to add;
+}
+
+void DpPolyPathConfig::set_sample_level(const size_t sample_level) {
+  _sample_level = sample_level;
+}
+
+void DpPolyPathConfig::set_sample_points_num_each_level(const size_t sample_points_num_each_level) {
+  _sample_points_num_each_level = sample_points_num_each_level;
+}
+
+void DpPolyPathConfig::set_step_length_max(const double step_length_max) {
+  _step_length_max = step_length_max;
+}
+
+void DpPolyPathConfig::set_step_length_min(const double step_length_min) {
+  _step_length_min = step_length_min;
+}
+
+void DpPolyPathConfig::set_lateral_sample_offset(const double lateral_sample_offset) {
+  _lateral_sample_offset = lateral_sample_offset;
+}
+
+void DpPolyPathConfig::set_lateral_adjust_coeff(const double lateral_adjust_coeff) {
+  _lateral_adjust_coeff = lateral_adjust_coeff;
+}
+
+void DpPolyPathConfig::set_eval_time_interval(const double eval_time_interval) {
+  _eval_time_interval = eval_time_interval;
+}
+
+size_t DpPolyPathConfig::sample_level() const {
+  return _sample_level;
+}
+
+size_t DpPolyPathConfig::sample_points_num_each_level() const {
+  return _sample_points_num_each_level;
+}
+
+double DpPolyPathConfig::step_length_max() const {
+  return _step_length_max;
+}
+
+double DpPolyPathConfig::step_length_min() const {
+  return _step_length_min;
+}
+
+double DpPolyPathConfig::lateral_sample_offset() const {
+  return _lateral_sample_offset;
+}
+
+double DpPolyPathConfig::lateral_adjust_coeff() const {
+  return _lateral_adjust_coeff;
+}
+
+double DpPolyPathConfig::eval_time_interval() const {
+  return _eval_time_interval;
+}
+
+} // namespace planning
+} // namespace apollo

modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file dp_poly_path_config.h
+ **/
+
+#ifndef MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_DP_POLY_PATH_CONFIG_H
+#define MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_DP_POLY_PATH_CONFIG_H
+
+#include "boost/property_tree/ptree.hpp"
+
+namespace apollo {
+namespace planning {
+
+class DpPolyPathConfig {
+ public:
+  DpPolyPathConfig() = default;
+  DpPolyPathConfig(const boost::property_tree::ptree &_ptree);
+  void set_sample_level(const size_t sample_level);
+  void set_sample_points_num_each_level(const size_t sample_points_num_each_level);
+  void set_step_length_max(const double step_length_max);
+  void set_step_length_min(const double step_length_min);
+  void set_lateral_sample_offset(const double lateral_sample_offset);
+  void set_lateral_adjust_coeff(const double lateral_adjust_coeff);
+  void set_eval_time_interval(const double eval_time_interval);
+
+  size_t sample_level() const;
+  size_t sample_points_num_each_level() const;
+  double step_length_max() const;
+  double step_length_min() const;
+  double lateral_sample_offset() const;
+  double lateral_adjust_coeff() const;
+  double eval_time_interval() const;
+
+ private:
+  // Sampler Config
+  size_t _sample_level = 8;
+  size_t _sample_points_num_each_level = 9;
+  double _step_length_max = 10.0;
+  double _step_length_min = 5.0;
+  double _lateral_sample_offset = 0.5;
+  double _lateral_adjust_coeff = 0.5;
+  // Trajectory Cost Config
+  double _eval_time_interval = 0.1;
+
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_DP_POLY_PATH_CONFIG_H

modules/planning/optimizer/dp_poly_path/dp_road_graph.cpp

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file dp_road_graph.h
+ **/
+
+#include "modules/planning/optimizer/dp_poly_path/dp_road_graph.h"
+
+#include "modules/planning/common/path/frenet_frame_path.h"
+#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
+#include "modules/planning/math/sl_analytic_transformation.h"
+#include "modules/planning/math/double.h"
+#include "modules/planning/optimizer/dp_poly_path/path_sampler.h"
+#include "modules/planning/optimizer/dp_poly_path/trajectory_cost.h"
+#include "modules/common/proto/path_point.pb.h"
+#include "modules/common/proto/error_code.pb.h"
+#include "modules/common/log.h"
+#include "modules/planning/math/sl_analytic_transformation.h"
+
+namespace apollo {
+namespace planning {
+
+DpRoadGraph::DpRoadGraph(
+    const DpPolyPathConfig &config,
+    const ::apollo::common::TrajectoryPoint &init_point) :
+    _config(config),
+    _init_point(init_point) {
+}
+
+::apollo::common::ErrorCode DpRoadGraph::find_tunnel(
+    const DataCenter &data_center,
+    const ReferenceLine &reference_line,
+    DecisionData *const decision_data,
+    PathData *const path_data) {
+  CHECK_NOTNULL(path_data);
+  ::apollo::common::ErrorCode ret = ::apollo::common::ErrorCode::PLANNING_OK;
+  if (init(reference_line) != ::apollo::common::ErrorCode::PLANNING_OK) {
+    AERROR << "Fail to init dp road graph!";
+    return ::apollo::common::ErrorCode::PLANNING_ERROR_FAILED;
+  }
+  if (generate_graph(reference_line) != ::apollo::common::ErrorCode::PLANNING_OK) {
+    AERROR << "Fail to generate graph!";
+    return ::apollo::common::ErrorCode::PLANNING_ERROR_FAILED;
+  }
+  std::vector<size_t> min_cost_edges;
+  if (find_best_trajectory(data_center, reference_line, *decision_data, &min_cost_edges) !=
+      ::apollo::common::ErrorCode::PLANNING_OK) {
+    AERROR << "Fail to find best trajectory!";
+    return ::apollo::common::ErrorCode::PLANNING_ERROR_FAILED;
+  }
+  FrenetFramePath tunnel;
+  std::vector<::apollo::common::FrenetFramePoint> frenet_path;
+  frenet_path.push_back(_vertices[0].frame_point());
+  double accumulated_s = 0.0;
+  for (size_t i = 0; i < min_cost_edges.size(); ++i) {
+    GraphEdge edge = _edges[min_cost_edges[i]];
+    GraphVertex end_vertex = _vertices[edge.to_vertex()];
+    double step = 0.1; // TODO: get from config.
+    double current_s = step;
+    while (Double::compare(current_s, end_vertex.frame_point().s()) < 0.0) {
+      current_s += step;
+      const double l = edge.poly_path().evaluate(1, current_s);
+      const double dl = edge.poly_path().evaluate(2, current_s);
+      const double ddl = edge.poly_path().evaluate(3, current_s);
+      ::apollo::common::FrenetFramePoint frenet_frame_point;
+      frenet_frame_point.set_s(accumulated_s + current_s);
+      frenet_frame_point.set_l(l);
+      frenet_frame_point.set_dl(dl);
+      frenet_frame_point.set_ddl(ddl);
+      frenet_path.push_back(frenet_frame_point);
+    }
+    frenet_path.push_back(end_vertex.frame_point());
+    accumulated_s += end_vertex.frame_point().s();
+  }
+  tunnel.set_frenet_points(frenet_path);
+  path_data->set_frenet_path(tunnel);
+  // convert frenet path to path by reference line
+  std::vector<::apollo::common::PathPoint> path_points;
+  for (const ::apollo::common::FrenetFramePoint &frenet_point : frenet_path) {
+    Eigen::Vector2d xy_point;
+    ::apollo::common::SLPoint sl_point;
+    sl_point.set_s(frenet_point.s());
+    sl_point.set_l(frenet_point.l());
+
+    if (!reference_line.get_point_in_Cartesian_frame(sl_point, &xy_point)) {
+      AERROR << "Fail to convert sl point to xy point";
+      return ::apollo::common::ErrorCode::PLANNING_ERROR_FAILED;
+
+    }
+    ReferencePoint ref_point = reference_line.get_reference_point(frenet_point.s());
+    double theta = SLAnalyticTransformation::calculate_theta(
+        ref_point.heading(), ref_point.kappa(), frenet_point.l(), frenet_point.dl());
+    //TODO comment out unused variable
+    //double kappa = SLAnalyticTransformation::calculate_kappa(ref_point.kappa(),
+    //                                                         ref_point.dkappa(),
+    //                                                        frenet_point.l(),
+    //                                                         frenet_point.dl(),
+    //                                                         frenet_point.ddl());
+
+    ::apollo::common::PathPoint path_point;//(xy_point, theta, kappa, 0.0, 0.0, 0.0);
+    path_point.set_x(xy_point[0]);
+    path_point.set_y(xy_point[1]);
+    path_point.set_theta(theta);
+    path_point.set_dkappa(0.0);
+    path_point.set_ddkappa(0.0);
+    path_point.set_s(0.0);
+    path_point.set_z(0.0);
+
+    if (path_points.size() != 0) {
+      Eigen::Vector2d last(path_points.back().x(), path_points.back().y());
+      Eigen::Vector2d current(path_point.x(), path_point.y());
+      double distance = (last - current).norm();
+      path_point.set_s(path_points.back().s() + distance);
+    }
+    path_points.push_back(std::move(path_point));
+  }
+  *(path_data->mutable_path()->mutable_path_points()) = path_points;
+  return ret;
+}
+
+::apollo::common::ErrorCode DpRoadGraph::init(const ReferenceLine &reference_line) {
+  _vertices.clear();
+  _edges.clear();
+  Eigen::Vector2d xy_point(_init_point.path_point().x(), _init_point.path_point().y());
+
+  if (!reference_line.get_point_in_Frenet_frame(xy_point, &_init_sl_point)) {
+    AERROR << "Fail to map init point to sl coordinate!";
+    return ::apollo::common::ErrorCode::PLANNING_ERROR_FAILED;
+  }
+
+  ReferencePoint reference_point = reference_line.get_reference_point(_init_sl_point.s());
+
+  double init_dl = SLAnalyticTransformation().calculate_lateral_derivative(
+      reference_point.heading(), _init_point.path_point().theta(),
+      _init_sl_point.l(), reference_point.kappa());
+  double init_ddl = SLAnalyticTransformation().calculate_second_order_lateral_derivative(
+      reference_point.heading(), _init_point.path_point().theta(),
+      reference_point.kappa(), _init_point.path_point().kappa(),
+      reference_point.dkappa(), _init_sl_point.l());
+  ::apollo::common::FrenetFramePoint init_frenet_frame_point;
+  init_frenet_frame_point.set_s(_init_sl_point.s());
+  init_frenet_frame_point.set_l(_init_sl_point.l());
+  init_frenet_frame_point.set_dl(init_dl);
+  init_frenet_frame_point.set_ddl(init_ddl);
+
+  _vertices.emplace_back(init_frenet_frame_point, 0, 0);
+  _vertices.back().set_type(GraphVertex::Type::GRAPH_HEAD);
+  _vertices.back().set_accumulated_cost(0.0);
+  return ::apollo::common::ErrorCode::PLANNING_OK;
+}
+
+::apollo::common::ErrorCode DpRoadGraph::generate_graph(const ReferenceLine &reference_line) {
+  ::apollo::common::ErrorCode ret = ::apollo::common::ErrorCode::PLANNING_OK;
+  std::vector<std::vector<::apollo::common::SLPoint>> points;
+  PathSampler path_sampler(_config);
+  ret = path_sampler.sample(reference_line, _init_point, _init_sl_point, &points);
+  if (ret != ::apollo::common::ErrorCode::PLANNING_OK) {
+    AERROR << "Fail to sampling point with path sampler!";
+    return ::apollo::common::ErrorCode::PLANNING_ERROR_FAILED;
+  }
+
+  int vertex_num_previous_level = 1;
+  size_t accumulated_prev_level_size = 0;
+  size_t accumulated_index = 0;
+  for (size_t i = 0; i < points.size(); ++i) {
+    int vertex_num_current_level = 0;
+    ReferencePoint reference_point = reference_line.get_reference_point(points[i][0].s());
+    for (size_t j = 0; j < points[i].size(); ++j) {
+      if (!add_vertex(points[i][j], reference_point, i + 1)) {
+        continue;
+      }
+      bool is_connected = false;
+      for (int n = 0; n < vertex_num_previous_level; ++n) {
+        const size_t index_start = accumulated_prev_level_size + n;
+        const size_t index_end = accumulated_prev_level_size +
+            vertex_num_previous_level + vertex_num_current_level;
+        if (connect_vertex(index_start, index_end)) {
+          is_connected = true;
+        }
+      }
+      if (is_connected) {
+        ++vertex_num_current_level;
+        if (i + 1 == points.size()) {
+          _vertices.back().set_type(GraphVertex::Type::DEAD_END);
+        }
+        ++accumulated_index;
+      } else {
+        _vertices.pop_back();
+      }
+      if (vertex_num_current_level == 0) {
+        return ::apollo::common::ErrorCode::PLANNING_ERROR_FAILED;
+      }
+    }
+    accumulated_prev_level_size += vertex_num_previous_level;
+    vertex_num_previous_level = vertex_num_current_level;
+  }
+  return ret;
+}
+
+::apollo::common::ErrorCode DpRoadGraph::find_best_trajectory(
+    const DataCenter &data_center,
+    const ReferenceLine &reference_line,
+    const DecisionData &decision_data,
+    std::vector<size_t> *const min_cost_edges) {
+  CHECK_NOTNULL(min_cost_edges);
+  std::unordered_map<size_t, size_t> vertex_connect_table;
+  GraphVertex &head = _vertices[0];
+  head.set_accumulated_cost(0.0);
+  size_t best_trajectory_end_index = 0;
+  double min_trajectory_cost = std::numeric_limits<double>::max();
+  SpeedData heuristic_speed_data; // need to create
+  TrajectoryCost trajectory_cost(_config, heuristic_speed_data, decision_data);
+
+  for (size_t i = 0; i < _vertices.size(); ++i) {
+    const GraphVertex &vertex = _vertices[i];
+    const std::vector<size_t> edges = vertex.edges_out();
+    for (size_t j = 0; j < edges.size(); ++j) {
+      double start_s = vertex.frame_point().s();
+      double end_s = _vertices[_edges[j].to_vertex()].frame_point().s();
+      double cost = trajectory_cost.calculate(
+          _edges[j].poly_path(), start_s, end_s,
+          4.933, //TODO length change to vehicle_config
+          2.11, //TODO width change to vehicle_config
+          reference_line);
+      GraphVertex &vertex_end = _vertices[_edges[j].to_vertex()];
+      if (vertex_connect_table.find(vertex_end.index()) == vertex_connect_table.end()) {
+        vertex_end.set_accumulated_cost(vertex.accumulated_cost() + cost);
+        vertex_connect_table[vertex_end.index()] = vertex.index();
+      } else {
+        if (Double::compare(
+            vertex.accumulated_cost() + cost, vertex_end.accumulated_cost()) < 0) {
+          vertex_end.set_accumulated_cost(vertex.accumulated_cost() + cost);
+          vertex_connect_table[vertex_end.index()] = vertex.index();
+        }
+      }
+    }
+    if (vertex.is_dead_end()
+        && Double::compare(vertex.accumulated_cost(), min_trajectory_cost) <= 0) {
+      best_trajectory_end_index = vertex.index();
+      min_trajectory_cost = vertex.accumulated_cost();
+    }
+  }
+  while (best_trajectory_end_index != 0) {
+    min_cost_edges->push_back(best_trajectory_end_index);
+    best_trajectory_end_index = vertex_connect_table[best_trajectory_end_index];
+  }
+  min_cost_edges->push_back(0);
+  std::reverse(min_cost_edges->begin(), min_cost_edges->end());
+  return ::apollo::common::ErrorCode::PLANNING_OK;
+}
+
+bool DpRoadGraph::add_vertex(const ::apollo::common::SLPoint &sl_point,
+                             const ReferencePoint &reference_point, const size_t level) {
+  double kappa = reference_point.kappa();
+  double kappa_range_upper = 0.23;
+  double kappa_range_lower = -kappa_range_upper;
+
+  if (Double::compare(kappa * sl_point.l(), 1.0) == 0) {
+    kappa = 0.0;
+  } else {
+    kappa = kappa / (1 - kappa * sl_point.l());
+    if (kappa < kappa_range_lower || kappa > kappa_range_upper) {
+      //Invalid sample point
+      return false;
+    }
+    kappa = std::max(kappa_range_lower, kappa);
+    kappa = std::min(kappa_range_upper, kappa);
+  }
+
+  ::apollo::common::FrenetFramePoint frenet_frame_point;
+  frenet_frame_point.set_s(sl_point.s());
+  frenet_frame_point.set_l(sl_point.l());
+  frenet_frame_point.set_dl(0.0);
+  frenet_frame_point.set_ddl(0.0);
+
+  _vertices.emplace_back(frenet_frame_point, _vertices.size(), level);
+  return true;
+}
+
+bool DpRoadGraph::connect_vertex(const size_t start, const size_t end) {
+  GraphVertex &v_start = _vertices[start];
+  GraphVertex &v_end = _vertices[end];
+  QuinticPolynomialCurve1d curve(
+      v_start.frame_point().l(), v_start.frame_point().dl(), v_start.frame_point().ddl(),
+      v_end.frame_point().l(), v_end.frame_point().dl(), v_end.frame_point().ddl(),
+      v_end.frame_point().s() - v_start.frame_point().s());
+  v_start.add_out_vertex(end);
+  v_start.add_out_edge(_edges.size());
+
+  v_end.add_in_vertex(start);
+  v_end.add_in_edge(_edges.size());
+
+  _edges.emplace_back(start, end, v_start.level());
+  _edges.back().set_edge_index(_edges.size() - 1);
+  _edges.back().set_poly_path(curve);
+  return true;
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/optimizer/dp_poly_path/dp_road_graph.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file dp_road_graph.h
+ **/
+
+#ifndef MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_DP_ROAD_GRAPH_H
+#define MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_DP_ROAD_GRAPH_H
+
+#include <vector>
+#include "boost/property_tree/ptree.hpp"
+#include "modules/common/proto/error_code.pb.h"
+#include "modules/common/proto/path_point.pb.h"
+#include "modules/planning/common/path/path_data.h"
+#include "modules/planning/common/speed/speed_data.h"
+#include "modules/planning/common/trajectory/discretized_trajectory.h"
+#include "modules/planning/common/data_center.h"
+#include "modules/planning/optimizer/dp_poly_path/graph_edge.h"
+#include "modules/planning/optimizer/dp_poly_path/graph_vertex.h"
+#include "modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h"
+#include "modules/planning/common/decision_data.h"
+#include "modules/planning/reference_line/reference_point.h"
+#include "modules/planning/reference_line/reference_line.h"
+
+namespace apollo {
+namespace planning {
+
+class DpRoadGraph {
+ public:
+  explicit DpRoadGraph(
+      const DpPolyPathConfig &config,
+      const ::apollo::common::TrajectoryPoint &init_point);
+  ~DpRoadGraph() = default;
+  ::apollo::common::ErrorCode find_tunnel(
+      const DataCenter &data_center,
+      const ReferenceLine &reference_line,
+      DecisionData *const decision_data,
+      PathData *const path_data);
+ private:
+  ::apollo::common::ErrorCode init(const ReferenceLine &reference_line);
+  ::apollo::common::ErrorCode generate_graph(const ReferenceLine &reference_line);
+  ::apollo::common::ErrorCode find_best_trajectory(
+      const DataCenter &data_center,
+      const ReferenceLine &reference_line,
+      const DecisionData &decision_data,
+      std::vector<size_t> *const min_cost_edges);
+  bool add_vertex(const ::apollo::common::SLPoint &sl_point,
+                  const ReferencePoint &reference_point, const size_t level);
+  bool connect_vertex(const size_t start, const size_t end);
+ private:
+  DpPolyPathConfig _config;
+  ::apollo::common::TrajectoryPoint _init_point;
+  std::vector<GraphVertex> _vertices;
+  std::vector<GraphEdge> _edges;
+  ::apollo::common::SLPoint _init_sl_point;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_DP_ROAD_GRAPH_H

modules/planning/optimizer/dp_poly_path/graph_edge.cpp

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file graph_edge.cpp
+ **/
+
+#include <cstdio>
+#include "modules/planning/optimizer/dp_poly_path/graph_edge.h"
+
+namespace apollo {
+namespace planning {
+
+GraphEdge::GraphEdge() : GraphEdge(0, 0, 0) {
+}
+
+GraphEdge::GraphEdge(const size_t from_vertex, const size_t to_vertex, const size_t level) :
+    _edge_index(0),
+    _from_vertex(from_vertex),
+    _to_vertex(to_vertex),
+    _level(level),
+    _cost(0.0) {
+}
+
+void GraphEdge::set_from_vertex(const size_t from_vertex) {
+  _from_vertex = from_vertex;
+}
+
+void GraphEdge::set_to_vertex(const size_t to_vertex) {
+  _to_vertex = to_vertex;
+}
+
+void GraphEdge::set_level(const size_t level) {
+  _level = level;
+}
+
+void GraphEdge::set_path(const std::vector<::apollo::common::FrenetFramePoint> &path) {
+  _path = path;
+}
+
+void GraphEdge::set_poly_path(const QuinticPolynomialCurve1d &ploy_path) {
+  _poly_path = ploy_path;
+}
+
+void GraphEdge::set_edge_index(const size_t index) {
+  _edge_index = index;
+}
+
+void GraphEdge::set_cost(const double cost) {
+  _cost = cost;
+}
+
+size_t GraphEdge::from_vertex() const {
+  return _from_vertex;
+}
+
+size_t GraphEdge::to_vertex() const {
+  return _to_vertex;
+}
+
+size_t GraphEdge::level() const {
+  return _level;
+}
+
+const std::vector<::apollo::common::FrenetFramePoint> &GraphEdge::path() const {
+  return _path;
+}
+
+const QuinticPolynomialCurve1d &GraphEdge::poly_path() const {
+  return _poly_path;
+}
+
+size_t GraphEdge::edge_index() const {
+  return _edge_index;
+}
+
+double GraphEdge::cost() const {
+  return _cost;
+}
+
+} // planning
+} // adu
+
+/* vim: set expandtab ts=4 sw=4 sts=4 tw=100: */

modules/planning/optimizer/dp_poly_path/graph_edge.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file sgraph_edge.h
+ **/
+
+#ifndef MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_EDGE_H
+#define MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_EDGE_H
+
+#include <vector>
+
+#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
+#include "modules/common/proto/path_point.pb.h"
+
+namespace apollo {
+namespace planning {
+
+class GraphEdge {
+ public:
+  explicit GraphEdge();
+  explicit GraphEdge(const size_t from_vertex, const size_t to_vertex, const size_t level);
+  ~GraphEdge() = default;
+
+  void set_from_vertex(const size_t index);
+  void set_to_vertex(const size_t index);
+  void set_level(const size_t level);
+  void set_path(const std::vector<::apollo::common::FrenetFramePoint> &path);
+  void set_poly_path(const QuinticPolynomialCurve1d &ploy_path);
+  void set_edge_index(const size_t index);
+  void set_cost(const double cost);
+
+  size_t from_vertex() const;
+  size_t to_vertex() const;
+  size_t level() const;
+  const std::vector<::apollo::common::FrenetFramePoint> &path() const;
+  const QuinticPolynomialCurve1d &poly_path() const;
+  size_t edge_index() const;
+  double cost() const;
+
+ private:
+  size_t _edge_index;
+  size_t _from_vertex;
+  size_t _to_vertex;
+  size_t _level;
+  double _cost;
+  std::vector<::apollo::common::FrenetFramePoint> _path;
+  QuinticPolynomialCurve1d _poly_path;
+};
+
+}  // planning
+}  // apollo
+
+#endif  // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_EDGE_H

modules/planning/optimizer/dp_poly_path/graph_vertex.cpp

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file sgraph_vertex.cpp
+ **/
+
+#include "modules/planning/optimizer/dp_poly_path/graph_vertex.h"
+
+#include <limits>
+#include <sstream>
+
+namespace apollo {
+namespace planning {
+
+GraphVertex::GraphVertex(const ::apollo::common::FrenetFramePoint &frame_point,
+                         const size_t index, const size_t level) :
+    _frame_point(frame_point),
+    _index(index),
+    _level(level),
+    _type(Type::REGULAR),
+    _accumulated_cost(std::numeric_limits<double>::max()) {
+}
+
+const ::apollo::common::FrenetFramePoint &GraphVertex::frame_point() const {
+  return _frame_point;
+}
+
+::apollo::common::FrenetFramePoint *GraphVertex::mutable_frame_point() {
+  return &_frame_point;
+}
+
+void GraphVertex::set_type(const Type type) {
+  _type = type;
+}
+
+void GraphVertex::set_accumulated_cost(const double accumulated_cost) {
+  _accumulated_cost = accumulated_cost;
+}
+
+void GraphVertex::set_index(const size_t index) {
+  _index = index;
+}
+
+void GraphVertex::set_level(const size_t level) {
+  _level = level;
+}
+
+bool GraphVertex::is_graph_head() const {
+  return _type == Type::GRAPH_HEAD;
+}
+
+bool GraphVertex::is_dead_end() const {
+  return _type == Type::DEAD_END;
+}
+
+double GraphVertex::accumulated_cost() const {
+  return _accumulated_cost;
+}
+
+size_t GraphVertex::index() const {
+  return _index;
+}
+
+size_t GraphVertex::level() const {
+  return _level;
+}
+
+void GraphVertex::add_out_vertex(const size_t vertex_index) {
+  _vertices_out.push_back(vertex_index);
+}
+
+void GraphVertex::add_out_vertex(const std::vector<size_t> &vertices) {
+  _vertices_out.insert(_vertices_out.end(), vertices.begin(), vertices.end());
+}
+
+void GraphVertex::add_in_vertex(const size_t vertex_index) {
+  _vertices_in.push_back(vertex_index);
+}
+
+void GraphVertex::add_in_vertex(const std::vector<size_t> &vertices) {
+  _vertices_in.insert(_vertices_in.end(), vertices.begin(), vertices.end());
+}
+
+void GraphVertex::add_out_edge(const size_t edge_index) {
+  _edges_out.push_back(edge_index);
+}
+
+void GraphVertex::add_out_edge(const std::vector<size_t> &edges) {
+  _edges_out.insert(_edges_out.end(), edges.begin(), edges.end());
+}
+
+void GraphVertex::add_in_edge(const size_t edge_index) {
+  _edges_in.push_back(edge_index);
+}
+
+void GraphVertex::add_in_edge(const std::vector<size_t> &edges) {
+  _edges_in.insert(_edges_in.end(), edges.begin(), edges.end());
+}
+
+const std::vector<size_t> &GraphVertex::vertices_out() const {
+  return _vertices_out;
+}
+
+const std::vector<size_t> &GraphVertex::vertices_in() const {
+  return _vertices_in;
+}
+
+const std::vector<size_t> &GraphVertex::edges_out() const {
+  return _edges_out;
+}
+
+const std::vector<size_t> &GraphVertex::edges_in() const {
+  return _edges_in;
+}
+
+std::string GraphVertex::to_string() const {
+  std::ostringstream sout;
+  sout << "index = " << _index << "; accumulated_cost = " << _accumulated_cost
+       << "s = " << _frame_point.s() << "; l = " << _frame_point.l()
+       << "; is_dead_end = " << std::boolalpha << is_dead_end() << std::endl;
+  for (const auto &v : _vertices_out) {
+    sout << "_vertices_out[" << v << "] ";
+  }
+  sout.flush();
+  return sout.str();
+}
+
+}  // planning
+}  // apollo

modules/planning/optimizer/dp_poly_path/graph_vertex.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file graph_point.h
+ **/
+
+#ifndef MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_VERTEX_H
+#define MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_VERTEX_H
+
+#include <vector>
+
+#include "modules/common/proto/path_point.pb.h"
+
+namespace apollo {
+namespace planning {
+
+class GraphVertex {
+ public:
+  enum class Type {
+    GRAPH_HEAD,
+    REGULAR,
+    DEAD_END
+  };
+
+  explicit GraphVertex(const ::apollo::common::FrenetFramePoint &frame_point,
+                       const size_t index, const size_t level);
+  ~GraphVertex() = default;
+
+  const ::apollo::common::FrenetFramePoint &frame_point() const;
+  ::apollo::common::FrenetFramePoint *mutable_frame_point();
+
+  void set_type(const Type type);
+  void set_index(const size_t index);
+  void set_level(const size_t level);
+  void set_accumulated_cost(const double accumulated_cost);
+
+  bool is_graph_head() const;
+  bool is_dead_end() const;
+  double accumulated_cost() const;
+  size_t index() const;
+  size_t level() const;
+
+  const std::vector<size_t> &vertices_out() const;
+  const std::vector<size_t> &vertices_in() const;
+  const std::vector<size_t> &edges_out() const;
+  const std::vector<size_t> &edges_in() const;
+
+  std::string to_string() const;
+
+ public:
+  void add_in_vertex(const std::vector<size_t> &vertices);
+  void add_in_vertex(const size_t vertex_index);
+  void add_out_vertex(const size_t vertex_index);
+  void add_out_vertex(const std::vector<size_t> &vertices);
+  void add_in_edge(const size_t edge_index);
+  void add_in_edge(const std::vector<size_t> &edges);
+  void add_out_edge(const size_t edge_index);
+  void add_out_edge(const std::vector<size_t> &edges);
+
+ private:
+  ::apollo::common::FrenetFramePoint _frame_point;
+  size_t _index = 0;
+  size_t _level = 0;
+  Type _type = Type::REGULAR;
+
+  std::vector<size_t> _vertices_in;
+  std::vector<size_t> _vertices_out;
+  std::vector<size_t> _edges_in;
+  std::vector<size_t> _edges_out;
+
+  double _accumulated_cost = 0.0;
+};
+
+}  // planning
+}  // apollo
+
+#endif  // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_VERTEX_H

modules/planning/optimizer/dp_poly_path/path_sampler.cpp

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file sampler.cpp
+ **/
+
+#include "modules/planning/optimizer/dp_poly_path/path_sampler.h"
+
+#include "modules/planning/math/double.h"
+#include "modules/common/proto/error_code.pb.h"
+#include "modules/common/log.h"
+#include "modules/planning/reference_line/reference_line.h"
+
+namespace apollo {
+namespace planning {
+
+PathSampler::PathSampler(const DpPolyPathConfig &config) : _config(config) {
+}
+
+::apollo::common::ErrorCode PathSampler::sample(
+    const ReferenceLine &reference_line,
+    const ::apollo::common::TrajectoryPoint &init_point,
+    const ::apollo::common::SLPoint &init_sl_point,
+    std::vector<std::vector<::apollo::common::SLPoint>> *const points) {
+  CHECK_NOTNULL(points);
+  double step_length = init_point.v();
+  step_length = std::min(step_length, _config.step_length_max());
+  step_length = std::max(step_length, _config.step_length_min());
+  double center_l = init_sl_point.l();
+  double accumulated_s = init_sl_point.s();
+  for (size_t i = 0; i < _config.sample_level(); ++i) {
+    std::vector<::apollo::common::SLPoint> level_points;
+    if (center_l < _config.lateral_sample_offset()) {
+      center_l = 0.0;
+    } else {
+      center_l = center_l * _config.lateral_adjust_coeff();
+    }
+    accumulated_s += step_length;
+    double level_start_l = center_l
+        - _config.lateral_sample_offset() * ((_config.sample_points_num_each_level() - 1) >> 1);
+    for (size_t j = 0; j < _config.sample_points_num_each_level(); ++j) {
+      // TODO: the lateral value is incorrect
+
+      // TODO: no checker no protection
+      ::apollo::common::SLPoint sl_point;
+      sl_point.set_s(accumulated_s);
+      sl_point.set_l(level_start_l + _config.lateral_sample_offset());
+      if (reference_line.is_on_road(sl_point)) {
+        level_points.push_back(sl_point);
+      }
+    }
+    points->push_back(level_points);
+  }
+  return ::apollo::common::ErrorCode::PLANNING_OK;
+}
+} // namespace planning
+} // namespace apollo

modules/planning/optimizer/dp_poly_path/path_sampler.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file sampler.h
+ **/
+
+#ifndef MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_SAMPLER_H
+#define MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_SAMPLER_H
+
+#include "modules/common/proto/error_code.pb.h"
+#include "modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h"
+#include "modules/planning/common/data_center.h"
+#include "modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h"
+#include "modules/common/proto/path_point.pb.h"
+#include "modules/planning/reference_line/reference_line.h"
+
+namespace apollo {
+namespace planning {
+
+class PathSampler {
+ public:
+  explicit PathSampler(const DpPolyPathConfig &config);
+  ~PathSampler() = default;
+  ::apollo::common::ErrorCode sample(
+      const ReferenceLine &reference_line,
+      const ::apollo::common::TrajectoryPoint &init_point,
+      const ::apollo::common::SLPoint &init_sl_point,
+      std::vector<std::vector<::apollo::common::SLPoint>> *const points);
+ private:
+  DpPolyPathConfig _config;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_SAMPLER_H

modules/planning/optimizer/dp_poly_path/trajectory_cost.cpp

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file trjactory_cost.h
+ **/
+
+#include "modules/planning/optimizer/dp_poly_path/trajectory_cost.h"
+
+#include <cmath>
+
+#include "modules/planning/common/planning_gflags.h"
+#include "modules/common/math/vec2d.h"
+#include "modules/common/proto/path_point.pb.h"
+
+namespace apollo {
+namespace planning {
+
+TrajectoryCost::TrajectoryCost(
+    const DpPolyPathConfig &config,
+    const SpeedData &heuristic_speed_data,
+    const DecisionData &decision_data) :
+    _config(config),
+    _heuristic_speed_data(heuristic_speed_data) {
+  const auto &static_obstacles = decision_data.StaticObstacles();
+  const double total_time =
+      std::min(_heuristic_speed_data.total_time(), FLAGS_prediction_total_time);
+  _evaluate_times = static_cast<size_t>(std::floor(total_time / config.eval_time_interval()));
+
+  // Mapping Static obstacle
+  for (size_t i = 0; i < static_obstacles.size(); ++i) {
+    std::vector<::apollo::common::math::Box2d> obstacle_by_time;
+    ::apollo::common::TrajectoryPoint traj_point =
+        static_obstacles[i]->prediction_trajectories()[0].evaluate(0.0);
+    ::apollo::common::math::Vec2d center_point = {traj_point.path_point().x(), traj_point.path_point().y()};
+    ::apollo::common::math::Box2d obstacle_box = {center_point, traj_point.path_point().theta(),
+                                                  static_obstacles[i]->BoundingBox().length(),
+                                                  static_obstacles[i]->BoundingBox().width()};
+    for (size_t j = 0; i <= _evaluate_times; ++j) {
+      obstacle_by_time.push_back(obstacle_box);
+    }
+    _obstacle_trajectory.push_back(obstacle_by_time);
+    _obstacle_probability.push_back(1.0);
+  }
+
+  // Mapping dynamic obstacle
+  const auto &dynamic_obstacles = decision_data.DynamicObstacles();
+  for (size_t i = 0; i < dynamic_obstacles.size(); ++i) {
+    const auto &trajectories = dynamic_obstacles[i]->prediction_trajectories();
+    for (size_t j = 0; j < trajectories.size(); ++j) {
+      const auto &trajectory = trajectories[j];
+      std::vector<::apollo::common::math::Box2d> obstacle_by_time;
+      for (size_t time = 0; time <= _evaluate_times; ++time) {
+        TrajectoryPoint traj_point =
+            trajectory.evaluate(time * config.eval_time_interval());
+        ::apollo::common::math::Vec2d center_point = {traj_point.path_point().x(), traj_point.path_point().y()};
+        ::apollo::common::math::Box2d obstacle_box = {center_point, traj_point.path_point().theta(),
+                                                      static_obstacles[i]->BoundingBox().length(),
+                                                      static_obstacles[i]->BoundingBox().width()};
+        obstacle_by_time.push_back(obstacle_box);
+      }
+      _obstacle_trajectory.push_back(obstacle_by_time);
+      _obstacle_probability.push_back(trajectory.probability());
+    }
+  }
+  //CHECK(_obstacle_probability.size() == _obstacle_trajectory.size());
+}
+
+double TrajectoryCost::calculate(const QuinticPolynomialCurve1d &curve,
+                                 const double start_s, const double end_s,
+                                 const double length, const double width,
+                                 const ReferenceLine &reference_line) const {
+  double total_cost = 0.0;
+  for (size_t i = 0; i < _evaluate_times; ++i) {
+    double eval_time = i * _config.eval_time_interval();
+    SpeedPoint speed_point;
+    if (!_heuristic_speed_data.get_speed_point_with_time(eval_time, &speed_point)
+        || start_s <= speed_point.s()) {
+      continue;
+    }
+    if (speed_point.s() >= end_s) {
+      break;
+    }
+    double l = curve.evaluate(1, speed_point.s() - start_s);
+    total_cost += l; // need refine l cost;
+
+    ::apollo::common::math::Vec2d car_point = {speed_point.s(), l};
+    ReferencePoint reference_point = reference_line.get_reference_point(car_point.x());
+    ::apollo::common::math::Box2d car_box = {car_point, reference_point.heading(), length, width};
+    for (size_t j = 0; j < _obstacle_trajectory.size(); ++j) {
+      ::apollo::common::math::Box2d obstacle_box = _obstacle_trajectory[j][i];
+      double distance = car_box.DistanceTo(obstacle_box);
+      total_cost += distance * _obstacle_probability[j]; // obstacle cost
+    }
+  }
+  return total_cost;
+}
+
+}  // namespace planning
+}  // namespace apollo
\ No newline at end of file

modules/planning/optimizer/dp_poly_path/trajectory_cost.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file trjactory_cost.h
+ **/
+
+#ifndef MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_TRAJECTORY_COST_H
+#define MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_TRAJECTORY_COST_H
+
+#include <vector>
+
+#include "modules/planning/common/decision_data.h"
+#include "modules/planning/common/speed/speed_data.h"
+#include "modules/planning/common/obstacle.h"
+#include "modules/common/math/box2d.h"
+#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
+#include "modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h"
+#include "modules/planning/reference_line/reference_line.h"
+#include "dp_poly_path_config.h"
+
+namespace apollo {
+namespace planning {
+
+class TrajectoryCost {
+ public:
+  explicit TrajectoryCost(
+      const DpPolyPathConfig &config,
+      const SpeedData &heuristic_speed_data,
+      const DecisionData &decision_data);
+  double calculate(
+      const QuinticPolynomialCurve1d &curve,
+      const double start_s, const double end_s,
+      const double length, const double width,
+      const ReferenceLine &reference_line) const;
+ private:
+  DpPolyPathConfig _config;
+  SpeedData _heuristic_speed_data;
+  std::vector<std::vector<::apollo::common::math::Box2d>> _obstacle_trajectory;
+  std::vector<double> _obstacle_probability;
+  size_t _evaluate_times;
+};
+
+}  // planning
+}  // apollo
+
+#endif  // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_TRAJECTORY_COST_H