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 e2addc0e569a5ef478d1f90a18dd72df415ca635..6207d5e80edbef2078fc20aa9ce3ba98cb8524da 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 @@ -56,8 +56,8 @@ Status DpPolyPathOptimizer::Process(const SpeedData &speed_data, } CHECK_NOTNULL(path_decision); CHECK_NOTNULL(path_data); - DPRoadGraph dp_road_graph(config_, init_point, speed_data); - if (!dp_road_graph.FindPathTunnel(reference_line, path_data)) { + DPRoadGraph dp_road_graph(config_, reference_line, speed_data); + if (!dp_road_graph.FindPathTunnel(init_point, path_data)) { AERROR << "Failed to find tunnel in road graph"; return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph path generation"); } @@ -65,8 +65,7 @@ Status DpPolyPathOptimizer::Process(const SpeedData &speed_data, frame_->path_decision()->path_obstacles().Items(); IdDecisionList decision_list; if (!dp_road_graph.ComputeObjectDecision(*path_data, speed_data, - reference_line, path_obstacles, - &decision_list)) { + path_obstacles, &decision_list)) { AERROR << "Failed to make decision based on tunnel"; return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph decision "); } 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 144013fbf91de99314f07b83aa9e844a5e04e1e5..bdc09fc9f629bd7ec7c4f984df8fa1888ac63299 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc @@ -46,19 +46,24 @@ using apollo::common::ErrorCode; using apollo::common::Status; DPRoadGraph::DPRoadGraph(const DpPolyPathConfig &config, - const common::TrajectoryPoint &init_point, + const ReferenceLine &reference_line, const SpeedData &speed_data) - : config_(config), init_point_(init_point), speed_data_(speed_data) {} + : config_(config), + reference_line_(reference_line), + speed_data_(speed_data) {} -bool DPRoadGraph::FindPathTunnel(const ReferenceLine &reference_line, +bool DPRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point, PathData *const path_data) { CHECK_NOTNULL(path_data); - if (!Init(reference_line)) { - AERROR << "Fail to init dp road graph!"; + init_point_ = init_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 creat init_sl_point from : " << init_point.DebugString(); return false; } std::vector min_cost_path; - if (!Generate(reference_line, &min_cost_path)) { + if (!Generate(&min_cost_path)) { AERROR << "Fail to generate graph!"; return false; } @@ -97,13 +102,13 @@ bool DPRoadGraph::FindPathTunnel(const ReferenceLine &reference_line, 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()); + 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()); @@ -133,42 +138,11 @@ bool DPRoadGraph::FindPathTunnel(const ReferenceLine &reference_line, 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_)) { - AERROR << "Fail to map init point from cartesian to sl coordinate!"; - return false; - } - - 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()); - - 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); - - return true; -} - -bool DPRoadGraph::Generate(const ReferenceLine &reference_line, - std::vector *min_cost_path) { +bool DPRoadGraph::Generate(std::vector *min_cost_path) { CHECK(min_cost_path != nullptr); std::vector> path_waypoints; - if (!SamplePathWaypoints(reference_line, init_point_, &path_waypoints)) { + if (!SamplePathWaypoints(init_point_, &path_waypoints)) { AERROR << "Fail to sample path waypoints!"; return false; } @@ -178,7 +152,7 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line, const auto &vehicle_config = common::VehicleConfigHelper::instance()->GetConfig(); - TrajectoryCost trajectory_cost(config_, reference_line, + TrajectoryCost trajectory_cost(config_, reference_line_, vehicle_config.vehicle_param(), speed_data_); std::vector> graph_nodes(path_waypoints.size()); @@ -224,8 +198,7 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line, } bool DPRoadGraph::ComputeObjectDecision( - const PathData &path_data, const SpeedData &heuristic_speed_data, - const ReferenceLine &reference_line, + const PathData &path_data, const SpeedData &speed_data, const ConstPathObstacleList &path_obstacles, IdDecisionList *const decisions) { CHECK_NOTNULL(decisions); @@ -249,7 +222,7 @@ bool DPRoadGraph::ComputeObjectDecision( common::math::Vec2d{path_point.x(), path_point.y()}, path_point.theta(), adc_length, adc_width); common::SLPoint adc_sl; - if (!reference_line.get_point_in_frenet_frame( + if (!reference_line_.get_point_in_frenet_frame( {path_point.x(), path_point.y()}, &adc_sl)) { AERROR << "get_point_in_Frenet_frame error for ego vehicle " << path_point.x() << " " << path_point.y(); @@ -329,7 +302,7 @@ bool DPRoadGraph::ComputeObjectDecision( // Compute dynamic obstacle decision const double total_time = - std::min(heuristic_speed_data.total_time(), FLAGS_prediction_total_time); + std::min(speed_data.total_time(), FLAGS_prediction_total_time); std::size_t evaluate_times = static_cast( std::floor(total_time / config_.eval_time_interval())); for (const auto *path_obstacle : path_obstacles) { @@ -340,8 +313,7 @@ bool DPRoadGraph::ComputeObjectDecision( // list of Box2d for ego car given heuristic speed profile std::vector adc_by_time; - if (!ComputeBoundingBoxesForAdc(path_data.frenet_frame_path(), - reference_line, heuristic_speed_data, + if (!ComputeBoundingBoxesForAdc(path_data.frenet_frame_path(), speed_data, evaluate_times, &adc_by_time)) { AERROR << "fill_adc_by_time error"; } @@ -356,8 +328,7 @@ bool DPRoadGraph::ComputeObjectDecision( // if two lists of boxes collide if (obstacle_by_time.size() != adc_by_time.size()) { AINFO << "dynamic_obstacle_by_time size[" << obstacle_by_time.size() - << "] != adc_by_time[" << adc_by_time.size() - << "] from heuristic_speed_data"; + << "] != adc_by_time[" << adc_by_time.size() << "] from speed_data"; continue; } @@ -378,8 +349,7 @@ bool DPRoadGraph::ComputeObjectDecision( } bool DPRoadGraph::ComputeBoundingBoxesForAdc( - const FrenetFramePath &frenet_frame_path, - const ReferenceLine &reference_line, const SpeedData &heuristic_speed_data, + const FrenetFramePath &frenet_frame_path, const SpeedData &speed_data, const std::size_t evaluate_times, std::vector *adc_boxes) { CHECK(adc_boxes != nullptr); @@ -392,8 +362,7 @@ bool DPRoadGraph::ComputeBoundingBoxesForAdc( for (std::size_t i = 0; i < evaluate_times; ++i) { double time_stamp = i * config_.eval_time_interval(); common::SpeedPoint speed_point; - if (!heuristic_speed_data.get_speed_point_with_time(time_stamp, - &speed_point)) { + if (!speed_data.get_speed_point_with_time(time_stamp, &speed_point)) { AINFO << "get_speed_point_with_time for time_stamp[" << time_stamp << "]"; return false; } @@ -408,10 +377,10 @@ bool DPRoadGraph::ComputeBoundingBoxesForAdc( common::SLPoint sl_point; sl_point.set_s(s); sl_point.set_l(l); - reference_line.get_point_in_Cartesian_frame(sl_point, - &adc_position_cartesian); + reference_line_.get_point_in_Cartesian_frame(sl_point, + &adc_position_cartesian); - ReferencePoint reference_point = reference_line.get_reference_point(s); + ReferencePoint reference_point = reference_line_.get_reference_point(s); double one_minus_kappa_r_d = 1 - reference_point.kappa() * l; double delta_theta = std::atan2(dl, one_minus_kappa_r_d); @@ -425,7 +394,6 @@ bool DPRoadGraph::ComputeBoundingBoxesForAdc( } bool DPRoadGraph::SamplePathWaypoints( - const ReferenceLine &reference_line, const common::TrajectoryPoint &init_point, std::vector> *const points) { CHECK(points != nullptr); @@ -433,15 +401,15 @@ bool DPRoadGraph::SamplePathWaypoints( common::math::Vec2d init_cartesian_point(init_point.path_point().x(), init_point.path_point().y()); common::SLPoint init_sl_point; - if (!reference_line.get_point_in_frenet_frame(init_cartesian_point, - &init_sl_point)) { + if (!reference_line_.get_point_in_frenet_frame(init_cartesian_point, + &init_sl_point)) { AERROR << "Failed to get sl point from point " << init_cartesian_point.DebugString(); return false; } const double reference_line_length = - reference_line.map_path().accumulated_s().back(); + reference_line_.map_path().accumulated_s().back(); double level_distance = std::fmax(config_.step_length_min(), @@ -461,7 +429,7 @@ bool DPRoadGraph::SamplePathWaypoints( for (int32_t j = -num; j < num + 1; ++j) { double l = config_.lateral_sample_offset() * j; auto sl = common::util::MakeSLPoint(s, l); - if (reference_line.is_on_road(sl)) { + if (reference_line_.is_on_road(sl)) { level_points.push_back(sl); } } 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 a20096788d839199b8f9db017a6f9d10a30ebdcc..fc0c4e4054c33c59b20e36e5f30f7551642df5a1 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph.h +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.h @@ -42,17 +42,16 @@ namespace planning { class DPRoadGraph { public: explicit DPRoadGraph(const DpPolyPathConfig &config, - const common::TrajectoryPoint &init_point, + const ReferenceLine &reference_line, const SpeedData &speed_data); ~DPRoadGraph() = default; - bool FindPathTunnel(const ReferenceLine &reference_line, + bool FindPathTunnel(const common::TrajectoryPoint &init_point, PathData *const path_data); bool ComputeObjectDecision(const PathData &path_data, - const SpeedData &heuristic_speed_data, - const ReferenceLine &reference_line, + const SpeedData &speed_data, const ConstPathObstacleList &obstacles, IdDecisionList *const decisions); @@ -87,25 +86,21 @@ class DPRoadGraph { QuinticPolynomialCurve1d min_cost_curve; }; - bool Init(const ReferenceLine &reference_line); - - bool Generate(const ReferenceLine &reference_line, - std::vector *min_cost_path); + bool Generate(std::vector *min_cost_path); bool ComputeBoundingBoxesForAdc( - const FrenetFramePath &frenet_frame_path, - const ReferenceLine &reference_line, - const SpeedData &heuristic_speed_data, const std::size_t evaluate_times, + const FrenetFramePath &frenet_frame_path, const SpeedData &speed_data, + const std::size_t evaluate_times, std::vector *adc_by_time); bool SamplePathWaypoints( - const ReferenceLine &reference_line, const common::TrajectoryPoint &init_point, std::vector> *const points); private: DpPolyPathConfig config_; common::TrajectoryPoint init_point_; + const ReferenceLine &reference_line_; SpeedData speed_data_; common::SLPoint init_sl_point_; }; 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 aaa3c3f2c1a09dfa595096637ab3265b11c0a454..3b6bc559386869ff3604ca01de2e705c387fbd73 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 @@ -82,9 +82,9 @@ class DpRoadGraphTest : public PlanningTestBase { }; TEST_F(DpRoadGraphTest, speed_road_graph) { - DPRoadGraph road_graph(dp_poly_path_config_, init_point_, speed_data_); + DPRoadGraph road_graph(dp_poly_path_config_, *reference_line_, speed_data_); ASSERT_TRUE(reference_line_ != nullptr); - bool result = road_graph.FindPathTunnel(*reference_line_, &path_data_); + bool result = road_graph.FindPathTunnel(init_point_, &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());