diff --git a/modules/planning/optimizer/dp_poly_path/BUILD b/modules/planning/optimizer/dp_poly_path/BUILD new file mode 100644 index 0000000000000000000000000000000000000000..857cea183a29265bf76f706463b5297b433624ec --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/BUILD @@ -0,0 +1,38 @@ +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", + ], +) diff --git a/modules/planning/optimizer/dp_poly_path/dp_poly_path_config.cpp b/modules/planning/optimizer/dp_poly_path/dp_poly_path_config.cpp new file mode 100644 index 0000000000000000000000000000000000000000..179fbe325a8c576e027eb61c65d59f480c57f748 --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/dp_poly_path_config.cpp @@ -0,0 +1,87 @@ +/****************************************************************************** + * 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 diff --git a/modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h b/modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h new file mode 100644 index 0000000000000000000000000000000000000000..8e730c7e24087e9329726644638522d9aae5b4ee --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/dp_poly_path_config.h @@ -0,0 +1,65 @@ +/****************************************************************************** + * 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 diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cpp b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cpp new file mode 100644 index 0000000000000000000000000000000000000000..497291c98f7c84bc4b0a7fe3c2ac084ca9b0f45e --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cpp @@ -0,0 +1,314 @@ +/****************************************************************************** + * 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 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> 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 *const min_cost_edges) { + CHECK_NOTNULL(min_cost_edges); + std::unordered_map 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::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 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 diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph.h b/modules/planning/optimizer/dp_poly_path/dp_road_graph.h new file mode 100644 index 0000000000000000000000000000000000000000..f95be0b4a0f99ed0e6b3ad6e9bb1372ab7b5d786 --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.h @@ -0,0 +1,75 @@ +/****************************************************************************** + * 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 +#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 *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 _vertices; + std::vector _edges; + ::apollo::common::SLPoint _init_sl_point; +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_DP_ROAD_GRAPH_H diff --git a/modules/planning/optimizer/dp_poly_path/graph_edge.cpp b/modules/planning/optimizer/dp_poly_path/graph_edge.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c6ceb0070a8e3fdfdda196085ea1d9bd40f18b07 --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/graph_edge.cpp @@ -0,0 +1,97 @@ +/****************************************************************************** + * 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 +#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: */ diff --git a/modules/planning/optimizer/dp_poly_path/graph_edge.h b/modules/planning/optimizer/dp_poly_path/graph_edge.h new file mode 100644 index 0000000000000000000000000000000000000000..403915b053fc94efd230322a23836a9fd5592b83 --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/graph_edge.h @@ -0,0 +1,67 @@ +/****************************************************************************** + * 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 + +#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 diff --git a/modules/planning/optimizer/dp_poly_path/graph_vertex.cpp b/modules/planning/optimizer/dp_poly_path/graph_vertex.cpp new file mode 100644 index 0000000000000000000000000000000000000000..84d52e8125546d5c93c838c79708b21897db6f4e --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/graph_vertex.cpp @@ -0,0 +1,143 @@ +/****************************************************************************** + * 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 +#include + +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::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 &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 &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 &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 &edges) { + _edges_in.insert(_edges_in.end(), edges.begin(), edges.end()); +} + +const std::vector &GraphVertex::vertices_out() const { + return _vertices_out; +} + +const std::vector &GraphVertex::vertices_in() const { + return _vertices_in; +} + +const std::vector &GraphVertex::edges_out() const { + return _edges_out; +} + +const std::vector &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 diff --git a/modules/planning/optimizer/dp_poly_path/graph_vertex.h b/modules/planning/optimizer/dp_poly_path/graph_vertex.h new file mode 100644 index 0000000000000000000000000000000000000000..43ce0abf4736a35ed2ed779eb528bd341655d400 --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/graph_vertex.h @@ -0,0 +1,91 @@ +/****************************************************************************** + * 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 + +#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 &vertices_out() const; + const std::vector &vertices_in() const; + const std::vector &edges_out() const; + const std::vector &edges_in() const; + + std::string to_string() const; + + public: + void add_in_vertex(const std::vector &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 &vertices); + void add_in_edge(const size_t edge_index); + void add_in_edge(const std::vector &edges); + void add_out_edge(const size_t edge_index); + void add_out_edge(const std::vector &edges); + + private: + ::apollo::common::FrenetFramePoint _frame_point; + size_t _index = 0; + size_t _level = 0; + Type _type = Type::REGULAR; + + std::vector _vertices_in; + std::vector _vertices_out; + std::vector _edges_in; + std::vector _edges_out; + + double _accumulated_cost = 0.0; +}; + +} // planning +} // apollo + +#endif // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_GRAPH_VERTEX_H diff --git a/modules/planning/optimizer/dp_poly_path/path_sampler.cpp b/modules/planning/optimizer/dp_poly_path/path_sampler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..683931cd5473a1e49f8af3931a2039d9e385cb0a --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/path_sampler.cpp @@ -0,0 +1,71 @@ +/****************************************************************************** + * 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> *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 diff --git a/modules/planning/optimizer/dp_poly_path/path_sampler.h b/modules/planning/optimizer/dp_poly_path/path_sampler.h new file mode 100644 index 0000000000000000000000000000000000000000..d15a1af2d2b3808148e1d1c94596029e18bb294c --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/path_sampler.h @@ -0,0 +1,50 @@ +/****************************************************************************** + * 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> *const points); + private: + DpPolyPathConfig _config; +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_SAMPLER_H diff --git a/modules/planning/optimizer/dp_poly_path/trajectory_cost.cpp b/modules/planning/optimizer/dp_poly_path/trajectory_cost.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8db4ff5249675a0f3ccf1c9a88960aea72e46a3c --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/trajectory_cost.cpp @@ -0,0 +1,113 @@ +/****************************************************************************** + * 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 + +#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(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 diff --git a/modules/planning/optimizer/dp_poly_path/trajectory_cost.h b/modules/planning/optimizer/dp_poly_path/trajectory_cost.h new file mode 100644 index 0000000000000000000000000000000000000000..09bfdd5d02d21888040e413ee8b18bee64ef42d6 --- /dev/null +++ b/modules/planning/optimizer/dp_poly_path/trajectory_cost.h @@ -0,0 +1,60 @@ +/****************************************************************************** + * 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 + +#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> _obstacle_trajectory; + std::vector _obstacle_probability; + size_t _evaluate_times; +}; + +} // planning +} // apollo + +#endif // MODULES_PLANNING_OPTIMIZER_DP_POLY_PATH_TRAJECTORY_COST_H