implemented dp algorithm in dp_road_graph

modules/planning/common/path/path_data.h

@@ -14,8 +14,7 @@
  * limitations under the License.
  *****************************************************************************/
 
-/**
- * @file path_data.h
+/** * @file path_data.h
  **/
 
 #ifndef MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_

modules/planning/conf/planning_config.pb.txt

 planner_type: EM
 em_planner_config {
-#  optimizer: DP_POLY_PATH_OPTIMIZER
+  optimizer: DP_POLY_PATH_OPTIMIZER
 #  optimizer: DP_ST_SPEED_OPTIMIZER
-  optimizer: QP_SPLINE_PATH_OPTIMIZER
+#  optimizer: QP_SPLINE_PATH_OPTIMIZER
   optimizer: QP_SPLINE_ST_SPEED_OPTIMIZER
-}
\ No newline at end of file
+}

modules/planning/optimizer/dp_poly_path/BUILD

@@ -6,15 +6,11 @@ cc_library(
     name = "dp_poly_path",
     srcs = [
         "dp_road_graph.cc",
-        "graph_edge.cc",
-        "graph_vertex.cc",
         "path_sampler.cc",
         "trajectory_cost.cc",
     ],
     hdrs = [
         "dp_road_graph.h",
-        "graph_edge.h",
-        "graph_vertex.h",
         "path_sampler.h",
         "trajectory_cost.h",
     ],

modules/planning/optimizer/dp_poly_path/dp_road_graph.cc

@@ -46,9 +46,7 @@ using apollo::common::Status;
 DpRoadGraph::DpRoadGraph(const DpPolyPathConfig &config,
                          const ::apollo::common::TrajectoryPoint &init_point,
                          const SpeedData &speed_data)
-  : _config(config),
-    _init_point(init_point),
-    _heuristic_speed_data(speed_data) {}
+    : _config(config), _init_point(init_point), _speed_data(speed_data) {}
 
 bool DpRoadGraph::find_tunnel(const ReferenceLine &reference_line,
                               DecisionData *const decision_data,
@@ -59,37 +57,31 @@ bool DpRoadGraph::find_tunnel(const ReferenceLine &reference_line,
     AERROR << "Fail to init dp road graph!";
     return false;
   }
-  if (!generate_graph(reference_line).ok()) {
+  std::vector<DpNode> min_cost_path;
+  if (!generate_graph(reference_line, decision_data, &min_cost_path)) {
     AERROR << "Fail to generate graph!";
     return false;
   }
-  std::vector<uint32_t> min_cost_edges;
-  if (!find_best_trajectory(reference_line, *decision_data, &min_cost_edges)
-      .ok()) {
-    AERROR << "Fail to find best trajectory!";
-    return false;
-  }
   std::vector<common::FrenetFramePoint> frenet_path;
-  double accumulated_s = _init_sl_point.s();
-
-  for (std::size_t i = 0; i < min_cost_edges.size(); ++i) {
-    GraphEdge edge = _edges[min_cost_edges[i]];
-    GraphVertex end_vertex = _vertices[edge.to_vertex()];
-    GraphVertex start_vertex = _vertices[edge.from_vertex()];
-    double path_length = end_vertex.frame_point().s() - start_vertex.frame_point().s();
+  double accumulated_s = _init_point.path_point().s();
+  const double path_resolution = _config.path_resolution();
+  for (size_t i = 1; i < min_cost_path.size(); ++i) {
+    const auto &prev_node = min_cost_path[i - 1];
+    const auto &cur_node = min_cost_path[i];
+    const double path_length = cur_node.sl_point.s() - prev_node.sl_point.s();
     double current_s = 0.0;
-    while (Double::compare(current_s, path_length) < 0) {
-      const double l = edge.poly_path().evaluate(0, current_s);
-      const double dl = edge.poly_path().evaluate(1, current_s);
-      const double ddl = edge.poly_path().evaluate(2, current_s);
+    const auto &curve = cur_node.min_cost_curve;
+    while (Double::compare(current_s, path_length) < 0.0) {
+      const double l = curve.evaluate(0, current_s);
+      const double dl = curve.evaluate(1, current_s);
+      const double ddl = curve.evaluate(2, current_s);
       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);
-      current_s += _config.path_resolution();
-      AERROR << "sl point " << frenet_frame_point.s() << "\t" << l << std::endl;
+      current_s += path_resolution;
     }
     accumulated_s += path_length;
   }
@@ -97,20 +89,24 @@ bool DpRoadGraph::find_tunnel(const ReferenceLine &reference_line,
   path_data->set_frenet_path(tunnel);
   // convert frenet path to cartesian path by reference line
   std::vector<::apollo::common::PathPoint> path_points;
-  for (const common::FrenetFramePoint& frenet_point : frenet_path) {
+  for (const common::FrenetFramePoint &frenet_point : frenet_path) {
     common::SLPoint sl_point;
     common::math::Vec2d cartesian_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, &cartesian_point)) {
+    if (!reference_line.get_point_in_Cartesian_frame(sl_point,
+                                                     &cartesian_point)) {
       AERROR << "Fail to convert sl point to xy point";
       return false;
     }
-    ReferencePoint ref_point = reference_line.get_reference_point(frenet_point.s());
+    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());
-    double kappa = SLAnalyticTransformation::calculate_kappa(ref_point.kappa(),
-                   ref_point.dkappa(), frenet_point.l(), frenet_point.dl(), frenet_point.ddl());
+        ref_point.heading(), ref_point.kappa(), frenet_point.l(),
+        frenet_point.dl());
+    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;
     path_point.set_x(cartesian_point.x());
     path_point.set_y(cartesian_point.y());
@@ -135,8 +131,6 @@ bool DpRoadGraph::find_tunnel(const ReferenceLine &reference_line,
 }
 
 bool DpRoadGraph::init(const ReferenceLine &reference_line) {
-  _vertices.clear();
-  _edges.clear();
   common::math::Vec2d xy_point(_init_point.path_point().x(),
                                _init_point.path_point().y());
 
@@ -146,186 +140,84 @@ bool DpRoadGraph::init(const ReferenceLine &reference_line) {
   }
 
   ReferencePoint reference_point =
-    reference_line.get_reference_point(_init_sl_point.s());
+      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());
+      _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());
   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 true;
 }
 
-Status DpRoadGraph::generate_graph(const ReferenceLine &reference_line) {
+bool DpRoadGraph::generate_graph(const ReferenceLine &reference_line,
+                                 DecisionData *const decision_data,
+                                 std::vector<DpNode> *min_cost_path) {
+  if (!min_cost_path) {
+    AERROR << "the provided min_cost_path is null";
+    return false;
+  }
   std::vector<std::vector<common::SLPoint>> points;
   PathSampler path_sampler(_config);
   if (!path_sampler.sample(reference_line, _init_point, _init_sl_point,
                            &points)) {
-    const std::string msg = "Fail to sampling point with path sampler!";
-    AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
-  }
-
-  int vertex_num_previous_level = 1;
-  uint32_t accumulated_prev_level_size = 0;
-  uint32_t accumulated_index = 0;
-  for (uint32_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 (uint32_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 uint32_t index_start = accumulated_prev_level_size + n;
-        const uint32_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 Status(ErrorCode::PLANNING_ERROR, "DpRoadGraph::generate_graph");
-      }
-    }
-    accumulated_prev_level_size += vertex_num_previous_level;
-    vertex_num_previous_level = vertex_num_current_level;
+    AERROR << "Fail to sampling point with path sampler!";
+    return false;
   }
-  return Status::OK();
-}
-
-Status DpRoadGraph::find_best_trajectory(
-  const ReferenceLine &reference_line, const DecisionData &decision_data,
-  std::vector<uint32_t> *const min_cost_edges) {
-  CHECK_NOTNULL(min_cost_edges);
-  std::unordered_map<uint32_t, uint32_t> vertex_connect_table;
-  GraphVertex &head = _vertices[0];
-  head.set_accumulated_cost(0.0);
-  uint32_t best_trajectory_end_index = 0;
+  points.insert(points.begin(), std::vector<common::SLPoint>{_init_sl_point});
   const auto &vehicle_config =
-    common::VehicleConfigHelper::instance()->GetConfig();
-  double min_trajectory_cost = std::numeric_limits<double>::max();
-  TrajectoryCost trajectory_cost(_config, vehicle_config.vehicle_param(),
-                                 _heuristic_speed_data, decision_data);
-  for (uint32_t i = 0; i < _vertices.size(); ++i) {
-    const GraphVertex& vertex = _vertices[i];
-    const std::vector<uint32_t> edges = vertex.edges_out();
-    for (uint32_t j = 0; j < edges.size(); ++j) {
-      auto edge = _edges[edges[j]];
-      GraphVertex& vertex_end = _vertices[edge.to_vertex()];
-      double start_s = vertex.frame_point().s();
-      double end_s = vertex_end.frame_point().s();
-      double cost = trajectory_cost.calculate(_edges[j].poly_path(), start_s,
-                                              end_s, reference_line);
-      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();
-    }
-  }
-  std::vector<uint32_t> min_cost_vertex;
-  while (best_trajectory_end_index != 0) {
-    min_cost_vertex.push_back(best_trajectory_end_index);
-    best_trajectory_end_index = vertex_connect_table[best_trajectory_end_index];
-  }
-  min_cost_vertex.push_back(0);
-  std::reverse(min_cost_vertex.begin(), min_cost_vertex.end());
-  for (uint32_t i = 1; i < min_cost_vertex.size(); ++i) {
-    AINFO << "Path vertex index " << min_cost_vertex[i];
-    const std::vector<uint32_t> &edges =
-      _vertices[min_cost_vertex[i - 1]].edges_out();
-    for (const uint32_t edge_index : edges) {
-      if (_edges[edge_index].to_vertex() == min_cost_vertex[i]) {
-        min_cost_edges->push_back(edge_index);
-        AINFO << "Path edge index " << edge_index;
+      common::VehicleConfigHelper::instance()->GetConfig();
+  TrajectoryCost trajectory_cost(_config, reference_line,
+                                 vehicle_config.vehicle_param(), _speed_data,
+                                 *decision_data);
+  std::vector<std::vector<DpNode>> dp_nodes(points.size());
+  dp_nodes[0].push_back(DpNode(_init_sl_point, nullptr, 0.0));
+  const double zero_dl = 0.0;   // always use 0 dl
+  const double zero_ddl = 0.0;  // always use 0 ddl
+  for (std::size_t level = 1; level < points.size(); ++level) {
+    const auto &prev_dp_nodes = dp_nodes[level - 1];
+    const auto &level_points = points[level];
+    for (std::size_t i = 0; i < level_points.size(); ++i) {
+      const auto cur_sl_point = level_points[i];
+      dp_nodes[level].push_back(DpNode(cur_sl_point, nullptr));
+      auto *cur_node = &dp_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;
+        QuinticPolynomialCurve1d curve(prev_sl_point.l(), zero_dl, zero_ddl,
+                                       cur_sl_point.l(), zero_dl, zero_ddl,
+                                       cur_sl_point.s() - prev_sl_point.s());
+        const double cost = trajectory_cost.calculate(curve, prev_sl_point.s(),
+                                                      cur_sl_point.s()) +
+                            prev_dp_node->min_cost;
+        cur_node->update_cost(prev_dp_node, curve, cost);
       }
     }
   }
-  return Status::OK();
-}
-
-bool DpRoadGraph::add_vertex(const common::SLPoint & sl_point,
-                             const ReferencePoint & reference_point,
-                             const uint32_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);
+  // find best path
+  DpNode fake_head;
+  const auto &last_dp_nodes = dp_nodes.back();
+  for (std::size_t i = 0; i < last_dp_nodes.size(); ++i) {
+    const auto &cur_dp_node = last_dp_nodes[i];
+    fake_head.update_cost(&cur_dp_node, cur_dp_node.min_cost_curve,
+                          cur_dp_node.min_cost);
   }
-
-  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 uint32_t start, const uint32_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);
+  const auto *min_cost_node = &fake_head;
+  while (min_cost_node->min_cost_prev_node) {
+    min_cost_node = min_cost_node->min_cost_prev_node;
+    min_cost_path->push_back(*min_cost_node);
+  }
+  std::reverse(min_cost_path->begin(), min_cost_path->end());
   return true;
 }
 

modules/planning/optimizer/dp_poly_path/dp_road_graph.h

@@ -30,8 +30,7 @@
 #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/optimizer/dp_poly_path/graph_edge.h"
-#include "modules/planning/optimizer/dp_poly_path/graph_vertex.h"
+#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
 #include "modules/planning/proto/dp_poly_path_config.pb.h"
 #include "modules/planning/reference_line/reference_line.h"
 #include "modules/planning/reference_line/reference_point.h"
@@ -50,21 +49,44 @@ class DpRoadGraph {
                    PathData *const path_data);
 
  private:
+  /**
+   * an private inner stuct for the dp algorithm
+   */
+  struct DpNode {
+   public:
+    DpNode() = default;
+    DpNode(const common::SLPoint point_sl, const DpNode *node_prev)
+        : sl_point(point_sl), min_cost_prev_node(node_prev) {}
+    DpNode(const common::SLPoint point_sl, const DpNode *node_prev,
+           const double cost)
+        : sl_point(point_sl), min_cost_prev_node(node_prev), min_cost(cost) {}
+    bool update_cost(const DpNode *node_prev,
+                     const QuinticPolynomialCurve1d &curve, double cost) {
+      if (cost > min_cost) {
+        return false;
+      } else {
+        min_cost = cost;
+        min_cost_prev_node = node_prev;
+        min_cost_curve = curve;
+        return true;
+      }
+    }
+
+    common::SLPoint sl_point;
+    const DpNode *min_cost_prev_node = nullptr;
+    double min_cost = std::numeric_limits<double>::infinity();
+    QuinticPolynomialCurve1d min_cost_curve;
+  };
+
   bool init(const ReferenceLine &reference_line);
-  ::apollo::common::Status generate_graph(const ReferenceLine &reference_line);
-  ::apollo::common::Status find_best_trajectory(
-      const ReferenceLine &reference_line, const DecisionData &decision_data,
-      std::vector<uint32_t> *const min_cost_edges);
-  bool add_vertex(const common::SLPoint &sl_point,
-                  const ReferencePoint &reference_point, const uint32_t level);
-  bool connect_vertex(const uint32_t start, const uint32_t end);
+  bool generate_graph(const ReferenceLine &reference_line,
+                      DecisionData *const decision_data,
+                      std::vector<DpNode> *min_cost_path);
 
  private:
   DpPolyPathConfig _config;
   ::apollo::common::TrajectoryPoint _init_point;
-  SpeedData _heuristic_speed_data;
-  std::vector<GraphVertex> _vertices;
-  std::vector<GraphEdge> _edges;
+  SpeedData _speed_data;
   common::SLPoint _init_sl_point;
 };
 

modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc

@@ -85,9 +85,8 @@ TEST_F(DpRoadGraphTest, speed_road_graph) {
   ASSERT_TRUE(reference_line_ != nullptr);
   bool result =
       road_graph.find_tunnel(*reference_line_, &decision_data_, &path_data_);
-
   EXPECT_TRUE(result);
-  // export_sl_points(sampled_points_, "/tmp/points.txt");
+  export_path_data(path_data_, "/tmp/path.txt");
 }
 
 }  // namespace planning

modules/planning/optimizer/dp_poly_path/graph_edge.cc

-/******************************************************************************
- * 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 uint32_t from_vertex, const uint32_t to_vertex,
-                     const uint32_t level)
-    : _edge_index(0),
-      _from_vertex(from_vertex),
-      _to_vertex(to_vertex),
-      _level(level),
-      _cost(0.0) {}
-
-void GraphEdge::set_from_vertex(const uint32_t from_vertex) {
-  _from_vertex = from_vertex;
-}
-
-void GraphEdge::set_to_vertex(const uint32_t to_vertex) {
-  _to_vertex = to_vertex;
-}
-
-void GraphEdge::set_level(const uint32_t level) { _level = level; }
-
-void GraphEdge::set_path(
-    const std::vector<common::FrenetFramePoint> &path) {
-  _path = path;
-}
-
-void GraphEdge::set_poly_path(const QuinticPolynomialCurve1d &ploy_path) {
-  _poly_path = ploy_path;
-}
-
-void GraphEdge::set_edge_index(const uint32_t index) { _edge_index = index; }
-
-void GraphEdge::set_cost(const double cost) { _cost = cost; }
-
-uint32_t GraphEdge::from_vertex() const { return _from_vertex; }
-
-uint32_t GraphEdge::to_vertex() const { return _to_vertex; }
-
-uint32_t GraphEdge::level() const { return _level; }
-
-const std::vector<common::FrenetFramePoint> &GraphEdge::path() const {
-  return _path;
-}
-
-const QuinticPolynomialCurve1d &GraphEdge::poly_path() const {
-  return _poly_path;
-}
-
-uint32_t GraphEdge::edge_index() const { return _edge_index; }
-
-double GraphEdge::cost() const { return _cost; }
-
-}  // namespace planning
-}  // namespace apollo

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:
-  GraphEdge();
-  explicit GraphEdge(const uint32_t from_vertex,
-                     const uint32_t to_vertex,
-                     const uint32_t level);
-  ~GraphEdge() = default;
-
-  void set_from_vertex(const uint32_t index);
-  void set_to_vertex(const uint32_t index);
-  void set_level(const uint32_t level);
-  void set_path(const std::vector<common::FrenetFramePoint> &path);
-  void set_poly_path(const QuinticPolynomialCurve1d &ploy_path);
-  void set_edge_index(const uint32_t index);
-  void set_cost(const double cost);
-
-  uint32_t from_vertex() const;
-  uint32_t to_vertex() const;
-  uint32_t level() const;
-  const std::vector<common::FrenetFramePoint> &path() const;
-  const QuinticPolynomialCurve1d &poly_path() const;
-  uint32_t edge_index() const;
-  double cost() const;
-
- private:
-  uint32_t _edge_index;
-  uint32_t _from_vertex;
-  uint32_t _to_vertex;
-  uint32_t _level;
-  double _cost;
-  std::vector<common::FrenetFramePoint> _path;
-  QuinticPolynomialCurve1d _poly_path;
-};
-
-}  // namespace planning
-}  // namespace apollo
-
-#endif  // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_EDGE_H

modules/planning/optimizer/dp_poly_path/graph_vertex.cc

-/******************************************************************************
- * 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 <limits>
-#include <sstream>
-#include <string>
-#include "modules/planning/optimizer/dp_poly_path/graph_vertex.h"
-
-namespace apollo {
-namespace planning {
-
-GraphVertex::GraphVertex(const common::FrenetFramePoint &frame_point,
-                         const uint32_t index, const uint32_t level) :
-    _frame_point(frame_point),
-    _index(index),
-    _level(level),
-    _type(Type::REGULAR),
-    _accumulated_cost(std::numeric_limits<double>::max()) {
-}
-
-const common::FrenetFramePoint &GraphVertex::frame_point() const {
-  return _frame_point;
-}
-
-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 uint32_t index) {
-  _index = index;
-}
-
-void GraphVertex::set_level(const uint32_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;
-}
-
-uint32_t GraphVertex::index() const {
-  return _index;
-}
-
-uint32_t GraphVertex::level() const {
-  return _level;
-}
-
-void GraphVertex::add_out_vertex(const uint32_t vertex_index) {
-  _vertices_out.push_back(vertex_index);
-}
-
-void GraphVertex::add_out_vertex(const std::vector<uint32_t> &vertices) {
-  _vertices_out.insert(_vertices_out.end(), vertices.begin(), vertices.end());
-}
-
-void GraphVertex::add_in_vertex(const uint32_t vertex_index) {
-  _vertices_in.push_back(vertex_index);
-}
-
-void GraphVertex::add_in_vertex(const std::vector<uint32_t> &vertices) {
-  _vertices_in.insert(_vertices_in.end(), vertices.begin(), vertices.end());
-}
-
-void GraphVertex::add_out_edge(const uint32_t edge_index) {
-  _edges_out.push_back(edge_index);
-}
-
-void GraphVertex::add_out_edge(const std::vector<uint32_t> &edges) {
-  _edges_out.insert(_edges_out.end(), edges.begin(), edges.end());
-}
-
-void GraphVertex::add_in_edge(const uint32_t edge_index) {
-  _edges_in.push_back(edge_index);
-}
-
-void GraphVertex::add_in_edge(const std::vector<uint32_t> &edges) {
-  _edges_in.insert(_edges_in.end(), edges.begin(), edges.end());
-}
-
-const std::vector<uint32_t> &GraphVertex::vertices_out() const {
-  return _vertices_out;
-}
-
-const std::vector<uint32_t> &GraphVertex::vertices_in() const {
-  return _vertices_in;
-}
-
-const std::vector<uint32_t> &GraphVertex::edges_out() const {
-  return _edges_out;
-}
-
-const std::vector<uint32_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();
-}
-
-}  // namespace planning
-}  // namespace 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 <string>
-#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 common::FrenetFramePoint &frame_point,
-                       const uint32_t index, const uint32_t level);
-  ~GraphVertex() = default;
-
-  const common::FrenetFramePoint &frame_point() const;
-  common::FrenetFramePoint *mutable_frame_point();
-
-  void set_type(const Type type);
-  void set_index(const uint32_t index);
-  void set_level(const uint32_t level);
-  void set_accumulated_cost(const double accumulated_cost);
-
-  bool is_graph_head() const;
-  bool is_dead_end() const;
-  double accumulated_cost() const;
-  uint32_t index() const;
-  uint32_t level() const;
-
-  const std::vector<uint32_t> &vertices_out() const;
-  const std::vector<uint32_t> &vertices_in() const;
-  const std::vector<uint32_t> &edges_out() const;
-  const std::vector<uint32_t> &edges_in() const;
-
-  std::string to_string() const;
-
- public:
-  void add_in_vertex(const std::vector<uint32_t> &vertices);
-  void add_in_vertex(const uint32_t vertex_index);
-  void add_out_vertex(const uint32_t vertex_index);
-  void add_out_vertex(const std::vector<uint32_t> &vertices);
-  void add_in_edge(const uint32_t edge_index);
-  void add_in_edge(const std::vector<uint32_t> &edges);
-  void add_out_edge(const uint32_t edge_index);
-  void add_out_edge(const std::vector<uint32_t> &edges);
-
- private:
-  common::FrenetFramePoint _frame_point;
-  uint32_t _index = 0;
-  uint32_t _level = 0;
-  Type _type = Type::REGULAR;
-
-  std::vector<uint32_t> _vertices_in;
-  std::vector<uint32_t> _vertices_out;
-  std::vector<uint32_t> _edges_in;
-  std::vector<uint32_t> _edges_out;
-
-  double _accumulated_cost = 0.0;
-};
-
-}  // namespace planning
-}  // namespace apollo
-
-#endif  // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_VERTEX_H

modules/planning/optimizer/dp_poly_path/trajectory_cost.cc

@@ -31,10 +31,12 @@ namespace apollo {
 namespace planning {
 
 TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config,
+                               const ReferenceLine &reference_line,
                                const common::VehicleParam &vehicle_param,
                                const SpeedData &heuristic_speed_data,
                                const DecisionData &decision_data)
     : _config(config),
+      _reference_line(&reference_line),
       _vehicle_param(vehicle_param),
       _heuristic_speed_data(heuristic_speed_data) {
   const auto &static_obstacles = decision_data.StaticObstacles();
@@ -87,8 +89,8 @@ TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config,
 }
 
 double TrajectoryCost::calculate(const QuinticPolynomialCurve1d &curve,
-                                 const double start_s, const double end_s,
-                                 const ReferenceLine &reference_line) const {
+                                 const double start_s,
+                                 const double end_s) const {
   // const double length = _vehicle_param.length();
   // const double width = _vehicle_param.width();
   double total_cost = 0.0;
@@ -112,7 +114,8 @@ double TrajectoryCost::calculate(const QuinticPolynomialCurve1d &curve,
     // ::apollo::common::math::Box2d car_box = {
     //     car_point, reference_point.heading(), length, width};
     // for (uint32_t j = 0; j < _obstacle_trajectory.size(); ++j) {
-    //   ::apollo::common::math::Box2d obstacle_box = _obstacle_trajectory[j][i];
+    //   ::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
     // }

modules/planning/optimizer/dp_poly_path/trajectory_cost.h

@@ -39,15 +39,16 @@ namespace planning {
 class TrajectoryCost {
  public:
   explicit TrajectoryCost(const DpPolyPathConfig &config,
+                          const ReferenceLine &reference_line,
                           const common::VehicleParam &vehicle_param,
                           const SpeedData &heuristic_speed_data,
                           const DecisionData &decision_data);
   double calculate(const QuinticPolynomialCurve1d &curve, const double start_s,
-                   const double end_s,
-                   const ReferenceLine &reference_line) const;
+                   const double end_s) const;
 
  private:
   const DpPolyPathConfig _config;
+  const ReferenceLine *_reference_line = nullptr;
   const common::VehicleParam _vehicle_param;
   SpeedData _heuristic_speed_data;
   std::vector<std::vector<::apollo::common::math::Box2d>> _obstacle_trajectory;

modules/planning/optimizer/optimizer_test_base.cc

@@ -80,5 +80,26 @@ void OptimizerTestBase::export_sl_points(
   ofs.close();
 }
 
+void OptimizerTestBase::export_path_data(const PathData& path_data,
+                                         const std::string& filename) {
+  std::ofstream ofs(filename);
+  ofs << "s, l, dl, ddl, x, y, z" << std::endl;
+  const auto& frenet_path = path_data.frenet_frame_path();
+  const auto& discrete_path = path_data.path();
+  if (frenet_path.num_points() != discrete_path.num_of_points()) {
+    AERROR << "frenet_path and discrete path have different number of points";
+    return;
+  }
+  for (uint32_t i = 0; i < frenet_path.num_points(); ++i) {
+    const auto& frenet_point = frenet_path.point_at(i);
+    const auto& discrete_point = discrete_path.path_point_at(i);
+    ofs << frenet_point.s() << ", " << frenet_point.l() << ", "
+        << frenet_point.dl() << ", " << frenet_point.ddl() << discrete_point.x()
+        << ", " << discrete_point.y() << ", " << discrete_point.z()
+        << std::endl;
+  }
+  ofs.close();
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/optimizer/optimizer_test_base.h

@@ -63,6 +63,9 @@ class OptimizerTestBase : public ::testing::Test {
       const std::vector<std::vector<common::SLPoint>>& points,
       const std::string& filename);
 
+  static void export_path_data(const PathData& path_data,
+                               const std::string& filename);
+
  protected:
   DpPolyPathConfig dp_poly_path_config_;
   Frame* frame_ = nullptr;