From 9794e711690d98e43b20c53291dc1553003e194d Mon Sep 17 00:00:00 2001 From: Dong Li Date: Fri, 4 Aug 2017 15:17:10 -0700 Subject: [PATCH] change dp_road_graph to two steps --- .../dp_poly_path/dp_poly_path_optimizer.cc | 9 +++- .../optimizer/dp_poly_path/dp_road_graph.cc | 42 +++++++------------ .../optimizer/dp_poly_path/dp_road_graph.h | 31 +++++++------- .../dp_poly_path/dp_road_graph_test.cc | 3 +- .../optimizer/dp_poly_path/trajectory_cost.cc | 25 +---------- .../optimizer/dp_poly_path/trajectory_cost.h | 6 +-- 6 files changed, 42 insertions(+), 74 deletions(-) diff --git a/modules/planning/optimizer/dp_poly_path/dp_poly_path_optimizer.cc b/modules/planning/optimizer/dp_poly_path/dp_poly_path_optimizer.cc index cf6e7ff984..c27d74eb0b 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_poly_path_optimizer.cc +++ b/modules/planning/optimizer/dp_poly_path/dp_poly_path_optimizer.cc @@ -57,9 +57,14 @@ Status DpPolyPathOptimizer::Process(const SpeedData &speed_data, CHECK_NOTNULL(decision_data); CHECK_NOTNULL(path_data); DPRoadGraph dp_road_graph(config_, init_point, speed_data); - if (!dp_road_graph.FindPathTunnel(reference_line, decision_data, path_data)) { + if (!dp_road_graph.FindPathTunnel(reference_line, path_data)) { AERROR << "Failed to find tunnel in road graph"; - return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph failed"); + return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph path generation"); + } + if (!dp_road_graph.ComputeObjectdecision(*path_data, speed_data, + reference_line, decision_data)) { + AERROR << "Failed to make decision based on tunnel"; + return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph decision "); } return Status::OK(); } diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc index 097202f145..eaa47f46b9 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc @@ -51,16 +51,14 @@ DPRoadGraph::DPRoadGraph(const DpPolyPathConfig &config, : config_(config), init_point_(init_point), speed_data_(speed_data) {} bool DPRoadGraph::FindPathTunnel(const ReferenceLine &reference_line, - DecisionData *const decision_data, - PathData *const path_data) { - CHECK_NOTNULL(decision_data); + PathData *const path_data) { CHECK_NOTNULL(path_data); if (!Init(reference_line)) { AERROR << "Fail to init dp road graph!"; return false; } std::vector min_cost_path; - if (!Generate(reference_line, decision_data, &min_cost_path)) { + if (!Generate(reference_line, &min_cost_path)) { AERROR << "Fail to generate graph!"; return false; } @@ -130,20 +128,13 @@ bool DPRoadGraph::FindPathTunnel(const ReferenceLine &reference_line, } path_data->set_discretized_path(path_points); - // compute decision data - if (compute_object_decision_from_path(*path_data, speed_data_, reference_line, - decision_data)) { - AINFO << "Computing decision_data in dp path success"; - } else { - AINFO << "Computing decision_data in dp path fail"; - } - return true; } bool DPRoadGraph::Init(const ReferenceLine &reference_line) { - if (!reference_line.get_point_in_frenet_frame({init_point_.path_point().x(), - init_point_.path_point().y()}, &init_sl_point_)) { + if (!reference_line.get_point_in_frenet_frame( + {init_point_.path_point().x(), init_point_.path_point().y()}, + &init_sl_point_)) { AERROR << "Fail to map init point from cartesian to sl coordinate!"; return false; } @@ -171,8 +162,7 @@ bool DPRoadGraph::Init(const ReferenceLine &reference_line) { } bool DPRoadGraph::Generate(const ReferenceLine &reference_line, - DecisionData *const decision_data, - std::vector *min_cost_path) { + std::vector *min_cost_path) { if (!min_cost_path) { AERROR << "the provided min_cost_path is null"; return false; @@ -182,13 +172,13 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line, AERROR << "Fail to sampling point with path sampler!"; return false; } - path_waypoints.insert(path_waypoints.begin(), std::vector{init_sl_point_}); + path_waypoints.insert(path_waypoints.begin(), + std::vector{init_sl_point_}); const auto &vehicle_config_ = common::VehicleConfigHelper::instance()->GetConfig(); TrajectoryCost trajectory_cost(config_, reference_line, - vehicle_config_.vehicle_param(), speed_data_, - *decision_data); + vehicle_config_.vehicle_param(), speed_data_); std::vector> graph_nodes(path_waypoints.size()); graph_nodes[0].push_back(DPRoadGraphNode(init_sl_point_, nullptr, 0.0)); @@ -201,7 +191,7 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line, for (std::size_t i = 0; i < level_points.size(); ++i) { const auto cur_sl_point = level_points[i]; graph_nodes[level].push_back(DPRoadGraphNode(cur_sl_point, nullptr)); - auto& cur_node = graph_nodes[level].back(); + auto &cur_node = graph_nodes[level].back(); for (std::size_t j = 0; j < prev_dp_nodes.size(); ++j) { const auto *prev_dp_node = &prev_dp_nodes[j]; const auto prev_sl_point = prev_dp_node->sl_point; @@ -222,7 +212,7 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line, for (std::size_t i = 0; i < last_dp_nodes.size(); ++i) { const auto &cur_dp_node = last_dp_nodes[i]; fake_head.UpdateCost(&cur_dp_node, cur_dp_node.min_cost_curve, - cur_dp_node.min_cost); + cur_dp_node.min_cost); } const auto *min_cost_node = &fake_head; while (min_cost_node->min_cost_prev_node) { @@ -233,9 +223,10 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line, return true; } -bool DPRoadGraph::compute_object_decision_from_path( - const PathData &path_data, const SpeedData &heuristic_speed_data, - const ReferenceLine &reference_line, DecisionData *const decision_data) { +bool DPRoadGraph::ComputeObjectdecision(const PathData &path_data, + const SpeedData &heuristic_speed_data, + const ReferenceLine &reference_line, + DecisionData *const decision_data) { CHECK_NOTNULL(decision_data); // Compute static obstacle decision @@ -428,11 +419,10 @@ bool DPRoadGraph::SamplePathWaypoints( const ReferenceLine &reference_line, const common::TrajectoryPoint &init_point, std::vector> *const points) { - CHECK(points != nullptr); common::math::Vec2d init_cartesian_point(init_point.path_point().x(), - init_point.path_point().y()); + init_point.path_point().y()); common::SLPoint init_sl_point; if (!reference_line.get_point_in_frenet_frame(init_cartesian_point, &init_sl_point)) { diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph.h b/modules/planning/optimizer/dp_poly_path/dp_road_graph.h index 78f6794e17..3f507a7394 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph.h +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.h @@ -47,8 +47,13 @@ class DPRoadGraph { ~DPRoadGraph() = default; bool FindPathTunnel(const ReferenceLine &reference_line, - DecisionData *const decision_data, - PathData *const path_data); + PathData *const path_data); + + bool ComputeObjectdecision(const PathData &path_data, + const SpeedData &heuristic_speed_data, + const ReferenceLine &reference_line, + DecisionData *const decision_data); + private: /** * an private inner struct for the dp algorithm @@ -57,11 +62,12 @@ class DPRoadGraph { public: DPRoadGraphNode() = default; - DPRoadGraphNode(const common::SLPoint point_sl, const DPRoadGraphNode *node_prev) + DPRoadGraphNode(const common::SLPoint point_sl, + const DPRoadGraphNode *node_prev) : sl_point(point_sl), min_cost_prev_node(node_prev) {} - DPRoadGraphNode(const common::SLPoint point_sl, const DPRoadGraphNode *node_prev, - const double cost) + DPRoadGraphNode(const common::SLPoint point_sl, + const DPRoadGraphNode *node_prev, const double cost) : sl_point(point_sl), min_cost_prev_node(node_prev), min_cost(cost) {} void UpdateCost(const DPRoadGraphNode *node_prev, @@ -82,13 +88,7 @@ class DPRoadGraph { bool Init(const ReferenceLine &reference_line); bool Generate(const ReferenceLine &reference_line, - DecisionData *const decision_data, - std::vector *min_cost_path); - - bool compute_object_decision_from_path(const PathData &path_data, - const SpeedData &heuristic_speed_data, - const ReferenceLine &reference_line, - DecisionData *const decision_data); + std::vector *min_cost_path); bool fill_ego_by_time( const FrenetFramePath &frenet_frame_path, @@ -96,9 +96,10 @@ class DPRoadGraph { const SpeedData &heuristic_speed_data, int evaluate_times, std::vector<::apollo::common::math::Box2d> *ego_by_time); - bool SamplePathWaypoints(const ReferenceLine &reference_line, - const common::TrajectoryPoint &init_point, - std::vector> *const points); + bool SamplePathWaypoints( + const ReferenceLine &reference_line, + const common::TrajectoryPoint &init_point, + std::vector> *const points); private: DpPolyPathConfig config_; diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc b/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc index dc1cffb383..f66f125a3c 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc @@ -85,8 +85,7 @@ class DpRoadGraphTest : public PlanningTestBase { TEST_F(DpRoadGraphTest, speed_road_graph) { DPRoadGraph road_graph(dp_poly_path_config_, init_point_, speed_data_); ASSERT_TRUE(reference_line_ != nullptr); - bool result = - road_graph.FindPathTunnel(*reference_line_, &decision_data_, &path_data_); + bool result = road_graph.FindPathTunnel(*reference_line_, &path_data_); EXPECT_TRUE(result); EXPECT_EQ(648, path_data_.discretized_path().num_of_points()); EXPECT_EQ(648, path_data_.frenet_frame_path().number_of_points()); diff --git a/modules/planning/optimizer/dp_poly_path/trajectory_cost.cc b/modules/planning/optimizer/dp_poly_path/trajectory_cost.cc index 2056984f17..92532c342f 100644 --- a/modules/planning/optimizer/dp_poly_path/trajectory_cost.cc +++ b/modules/planning/optimizer/dp_poly_path/trajectory_cost.cc @@ -37,8 +37,7 @@ using TrajectoryPoint = ::apollo::common::TrajectoryPoint; TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config, const ReferenceLine &reference_line, const common::VehicleParam &vehicle_param, - const SpeedData &heuristic_speed_data, - const DecisionData &decision_data) + const SpeedData &heuristic_speed_data) : config_(config), reference_line_(&reference_line), vehicle_param_(vehicle_param), @@ -47,28 +46,6 @@ TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config, std::min(heuristic_speed_data_.total_time(), FLAGS_prediction_total_time); evaluate_times_ = static_cast( std::floor(total_time / config.eval_time_interval())); - - // Mapping Static obstacle - for (const auto ptr_static_obstacle : decision_data.StaticObstacles()) { - static_obstacle_boxes_.push_back( - ptr_static_obstacle->PerceptionBoundingBox()); - } - - // Mapping dynamic obstacle - for (const auto ptr_dynamic_obstacle : decision_data.DynamicObstacles()) { - const auto &trajectory = ptr_dynamic_obstacle->Trajectory(); - std::vector obstacle_by_time; - for (std::size_t time = 0; time < evaluate_times_ + 1; ++time) { - TrajectoryPoint traj_point = ptr_dynamic_obstacle->GetPointAtTime( - time * config.eval_time_interval()); - obstacle_by_time.push_back( - ptr_dynamic_obstacle->GetBoundingBox(traj_point)); - } - dynamic_obstacle_trajectory_.push_back(std::move(obstacle_by_time)); - dynamic_obstacle_probability_.push_back(trajectory.probability()); - } - CHECK(dynamic_obstacle_trajectory_.size() == - dynamic_obstacle_probability_.size()); } double TrajectoryCost::calculate(const QuinticPolynomialCurve1d &curve, diff --git a/modules/planning/optimizer/dp_poly_path/trajectory_cost.h b/modules/planning/optimizer/dp_poly_path/trajectory_cost.h index 661d504049..21dbe3365b 100644 --- a/modules/planning/optimizer/dp_poly_path/trajectory_cost.h +++ b/modules/planning/optimizer/dp_poly_path/trajectory_cost.h @@ -41,8 +41,7 @@ class TrajectoryCost { explicit TrajectoryCost(const DpPolyPathConfig &config, const ReferenceLine &reference_line, const common::VehicleParam &vehicle_param, - const SpeedData &heuristic_speed_data, - const DecisionData &decision_data); + const SpeedData &heuristic_speed_data); double calculate(const QuinticPolynomialCurve1d &curve, const double start_s, const double end_s) const; @@ -51,9 +50,6 @@ class TrajectoryCost { const ReferenceLine *reference_line_ = nullptr; const common::VehicleParam vehicle_param_; SpeedData heuristic_speed_data_; - std::vector<::apollo::common::math::Box2d> static_obstacle_boxes_; - std::vector> dynamic_obstacle_trajectory_; - std::vector dynamic_obstacle_probability_; uint32_t evaluate_times_; }; -- GitLab