diff --git a/modules/planning/common/BUILD b/modules/planning/common/BUILD index 010d07a0ea82f7faa13b9672a6e1d30cde416715..0e09a829b69c271feb16f33803a77589ce5f8c3b 100644 --- a/modules/planning/common/BUILD +++ b/modules/planning/common/BUILD @@ -7,6 +7,9 @@ cc_library( hdrs = [ "indexed_list.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], linkopts = [ "-lboost_thread", ], @@ -33,6 +36,9 @@ cc_library( hdrs = [ "indexed_queue.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ "//modules/common/util:map_util", ], @@ -66,6 +72,9 @@ cc_library( hdrs = [ "obstacle.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":indexed_list", "//modules/common/configs:vehicle_config_helper", @@ -101,6 +110,9 @@ cc_library( hdrs = [ "planning_context.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":planning_gflags", "//cyber/common:log", @@ -120,6 +132,9 @@ cc_library( hdrs = [ "path_decision.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":obstacle", "//modules/planning/reference_line", @@ -134,6 +149,9 @@ cc_library( hdrs = [ "planning_gflags.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ "//external:gflags", ], @@ -147,6 +165,9 @@ cc_library( hdrs = [ "reference_line_info.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":ego_info", ":path_decision", @@ -186,6 +207,9 @@ cc_library( hdrs = [ "speed_profile_generator.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":frame", ":reference_line_info", @@ -234,6 +258,9 @@ cc_library( hdrs = [ "change_lane_decider.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":planning_gflags", ":reference_line_info", @@ -247,6 +274,9 @@ cc_library( hdrs = [ "local_view.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ "//modules/localization/proto:localization_proto", "//modules/map/relative_map/proto:navigation_proto", @@ -266,6 +296,9 @@ cc_library( "frame.h", "frame_manager.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":local_view", ":change_lane_decider", @@ -315,6 +348,9 @@ cc_library( hdrs = [ "speed_limit.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ "//modules/common/math", "//modules/planning/proto:planning_proto", @@ -341,6 +377,9 @@ cc_library( hdrs = [ "ego_info.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":obstacle", "//cyber/common:log", @@ -361,6 +400,9 @@ cc_library( hdrs = [ "threshold.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":frame", ":speed_limit", @@ -398,6 +440,9 @@ cc_test( cc_library( name = "planning_common", + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ "threshold", ":ego_info", @@ -416,6 +461,9 @@ cc_library( hdrs = [ "trajectory_info.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ "//modules/planning/common/path:path_data", "//modules/planning/common/speed:speed_data", diff --git a/modules/planning/common/path/BUILD b/modules/planning/common/path/BUILD index a561a82b12fe2544caee00483ba517a00cc2738f..dff9e96bf63de5271fcd0a5530f42bcfda3a4d62 100644 --- a/modules/planning/common/path/BUILD +++ b/modules/planning/common/path/BUILD @@ -10,10 +10,13 @@ cc_library( hdrs = [ "discretized_path.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ - "//modules/common/proto:pnc_point_proto", "//modules/common", "//modules/common/math:linear_interpolation", + "//modules/common/proto:pnc_point_proto", "//modules/planning/common:planning_context", ], ) @@ -39,6 +42,9 @@ cc_library( hdrs = [ "frenet_frame_path.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ "//modules/common/math:linear_interpolation", "//modules/common/proto:pnc_point_proto", @@ -66,6 +72,9 @@ cc_library( hdrs = [ "path_data.h", ], + copts = [ + "-DMODULE_NAME=\\\"planning\\\"", + ], deps = [ ":discretized_path", ":frenet_frame_path", diff --git a/modules/planning/common/path/path_data.cc b/modules/planning/common/path/path_data.cc index 8abaefa83d24d1183389532b5cc8b0e27986f3b8..988a436893cf6012dc01207afd82e979d1450dc3 100644 --- a/modules/planning/common/path/path_data.cc +++ b/modules/planning/common/path/path_data.cc @@ -177,6 +177,7 @@ bool PathData::SLToXY(const FrenetFramePath &frenet_path, double theta = CartesianFrenetConverter::CalculateTheta( ref_point.heading(), ref_point.kappa(), frenet_point.l(), frenet_point.dl()); + ADEBUG << "frenet_point: " << frenet_point.ShortDebugString(); double kappa = CartesianFrenetConverter::CalculateKappa( ref_point.kappa(), ref_point.dkappa(), frenet_point.l(), frenet_point.dl(), frenet_point.ddl()); @@ -209,15 +210,23 @@ bool PathData::XYToSL(const DiscretizedPath &discretized_path, std::vector frenet_frame_points; const double max_len = reference_line_->Length(); for (const auto &path_point : discretized_path.path_points()) { - SLPoint sl_point; - if (!reference_line_->XYToSL({path_point.x(), path_point.y()}, &sl_point)) { - AERROR << "Fail to transfer cartesian point to frenet point."; - return false; + common::FrenetFramePoint frenet_point = + reference_line_->GetFrenetPoint(path_point); + if (!frenet_point.has_s()) { + SLPoint sl_point; + if (!reference_line_->XYToSL({path_point.x(), path_point.y()}, + &sl_point)) { + AERROR << "Fail to transfer cartesian point to frenet point."; + return false; + } + common::FrenetFramePoint frenet_point; + // NOTICE: does not set dl and ddl here. Add if needed. + frenet_point.set_s(std::max(0.0, std::min(sl_point.s(), max_len))); + frenet_point.set_l(sl_point.l()); + frenet_frame_points.push_back(std::move(frenet_point)); + continue; } - common::FrenetFramePoint frenet_point; - // NOTICE: does not set dl and ddl here. Add if needed. - frenet_point.set_s(std::max(0.0, std::min(sl_point.s(), max_len))); - frenet_point.set_l(sl_point.l()); + frenet_point.set_s(std::max(0.0, std::min(frenet_point.s(), max_len))); frenet_frame_points.push_back(std::move(frenet_point)); } *frenet_path = FrenetFramePath(std::move(frenet_frame_points)); diff --git a/modules/planning/reference_line/reference_line.cc b/modules/planning/reference_line/reference_line.cc index 7966b621900555b09c31a46594ac2116ead2ac69..bf021cb510aa9b2330fe1bcfa6333da6e35d4c0c 100644 --- a/modules/planning/reference_line/reference_line.cc +++ b/modules/planning/reference_line/reference_line.cc @@ -180,15 +180,19 @@ bool ReferenceLine::Shrink(const double s, double look_backward, } common::FrenetFramePoint ReferenceLine::GetFrenetPoint( - const common::TrajectoryPoint& traj_point) const { + const common::PathPoint& path_point) const { + if (reference_points_.empty()) { + return common::FrenetFramePoint(); + } + common::SLPoint sl_point; - XYToSL({traj_point.path_point().x(), traj_point.path_point().y()}, &sl_point); + XYToSL({path_point.x(), path_point.y()}, &sl_point); common::FrenetFramePoint frenet_frame_point; frenet_frame_point.set_s(sl_point.s()); frenet_frame_point.set_l(sl_point.l()); - const double theta = traj_point.path_point().theta(); - const double kappa = traj_point.path_point().kappa(); + const double theta = path_point.theta(); + const double kappa = path_point.kappa(); const double l = frenet_frame_point.l(); ReferencePoint ref_point = GetReferencePoint(frenet_frame_point.s()); diff --git a/modules/planning/reference_line/reference_line.h b/modules/planning/reference_line/reference_line.h index aeb93ca999b28bc3022e5cb4e55f3a582e71ed55..2ea3a2824aa665d69b3d6110cd4e8d11bcebd105 100644 --- a/modules/planning/reference_line/reference_line.h +++ b/modules/planning/reference_line/reference_line.h @@ -81,7 +81,7 @@ class ReferenceLine { ReferencePoint GetReferencePoint(const double s) const; common::FrenetFramePoint GetFrenetPoint( - const common::TrajectoryPoint& traj_point) const; + const common::PathPoint& path_point) const; std::vector GetReferencePoints(double start_s, double end_s) const; diff --git a/modules/planning/scenarios/side_pass/side_pass_stage.cc b/modules/planning/scenarios/side_pass/side_pass_stage.cc index ce543d0702e6a43c8e01892c921a0855ec9eae7d..8fac8e1ed641a2181275b8acd0e5c000c38c6bee 100644 --- a/modules/planning/scenarios/side_pass/side_pass_stage.cc +++ b/modules/planning/scenarios/side_pass/side_pass_stage.cc @@ -78,14 +78,13 @@ Stage::StageStatus SidePassBackup::Process( double lane_left_width = 0.0; double lane_right_width = 0.0; reference_line.GetLaneWidth(obstacle->PerceptionSLBoundary().start_s(), - &lane_left_width, - &lane_right_width); - double driving_width = std::max( - lane_left_width - obstacle->PerceptionSLBoundary().end_l(), - lane_right_width + obstacle->PerceptionSLBoundary().start_l()); + &lane_left_width, &lane_right_width); + double driving_width = + std::max(lane_left_width - obstacle->PerceptionSLBoundary().end_l(), + lane_right_width + obstacle->PerceptionSLBoundary().start_l()); driving_width = std::min(lane_left_width + lane_right_width, driving_width) - - FLAGS_static_decision_nudge_l_buffer; + FLAGS_static_decision_nudge_l_buffer; ADEBUG << "driving_width[" << driving_width << "]"; if (driving_width > kLBufferThreshold) { continue; @@ -244,7 +243,7 @@ Stage::StageStatus SidePassDetectSafety::Process( const auto adc_frenet_frame_point_ = reference_line_info.reference_line().GetFrenetPoint( - frame->PlanningStartPoint()); + frame->PlanningStartPoint().path_point()); bool trim_success = GetContext()->path_data_.LeftTrimWithRefS( adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l()); @@ -305,7 +304,7 @@ Stage::StageStatus SidePassPassObstacle::Process( const auto adc_frenet_frame_point_ = reference_line_info.reference_line().GetFrenetPoint( - frame->PlanningStartPoint()); + frame->PlanningStartPoint().path_point()); bool trim_success = GetContext()->path_data_.LeftTrimWithRefS( adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l()); diff --git a/modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.cc b/modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.cc index fccffbd5de13237d0a2e905c838de37779fdb499..0675511c12745a1a5ec9b9c7bba18e0dc58fa26c 100644 --- a/modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.cc +++ b/modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.cc @@ -56,7 +56,7 @@ Stage::StageStatus SidePassStopOnWaitPoint::Process( } const auto adc_frenet_frame_point_ = - reference_line.GetFrenetPoint(frame->PlanningStartPoint()); + reference_line.GetFrenetPoint(frame->PlanningStartPoint().path_point()); if (!GetContext()->path_data_.LeftTrimWithRefS(adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l())) { diff --git a/modules/planning/std_planning.cc b/modules/planning/std_planning.cc index c9630b112a411b360c5c7c3a55f3d18039abd6d6..620be40a75e9b56b72b1f556494672a411e546a7 100644 --- a/modules/planning/std_planning.cc +++ b/modules/planning/std_planning.cc @@ -301,6 +301,9 @@ void StdPlanning::RunOnce(const LocalView& local_view, status = Plan(start_timestamp, stitching_trajectory, trajectory_pb); + for (const auto& p : trajectory_pb->trajectory_point()) { + ADEBUG << p.DebugString(); + } const auto time_diff_ms = (Clock::NowInSeconds() - start_timestamp) * 1000; ADEBUG << "total planning time spend: " << time_diff_ms << " ms."; diff --git a/modules/planning/toolkits/deciders/side_pass_path_decider.cc b/modules/planning/toolkits/deciders/side_pass_path_decider.cc index 495ee97385c4962b4b40365fbf9b9a6275953eff..3403d97837451b69d4b51e59b32e9904c51bd2c0 100644 --- a/modules/planning/toolkits/deciders/side_pass_path_decider.cc +++ b/modules/planning/toolkits/deciders/side_pass_path_decider.cc @@ -43,7 +43,7 @@ using apollo::common::util::MakePointENU; using apollo::hdmap::HDMapUtil; constexpr double kRoadBuffer = 0.2; -constexpr double kObstacleLBuffer = 0.2; +constexpr double kObstacleLBuffer = 1.0; constexpr double kObstacleSBuffer = 0.5; constexpr double kSidePassPathLength = 50.0; @@ -76,7 +76,7 @@ Status SidePassPathDecider::Process( adc_planning_start_point_ = frame->PlanningStartPoint(); adc_frenet_frame_point_ = reference_line_info->reference_line().GetFrenetPoint( - frame->PlanningStartPoint()); + frame->PlanningStartPoint().path_point()); InitSolver(); nearest_obstacle_ = @@ -205,7 +205,9 @@ bool SidePassPathDecider::GeneratePath( for (size_t i = 0; i < fem_qp_->x().size(); ++i) { common::FrenetFramePoint frenet_frame_point; ADEBUG << "FrenetFramePath: s = " << accumulated_s - << ", l = " << fem_qp_->x()[i]; + << ", l = " << fem_qp_->x()[i] + << ", dl = " << fem_qp_->x_derivative()[i] + << ", ddl = " << fem_qp_->x_second_order_derivative()[i]; if (accumulated_s >= reference_line_info->reference_line().Length()) { break; } diff --git a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc index 3aca56ab53b871d21acc2a1fe74d5fb120411c2a..ccf0dabed7c5a5b1f5e572c4927a23a198fda315 100644 --- a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc +++ b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc @@ -211,7 +211,8 @@ QpPiecewiseJerkPathOptimizer::GetLateralBounds( Status QpPiecewiseJerkPathOptimizer::Process( const SpeedData& speed_data, const ReferenceLine& reference_line, const common::TrajectoryPoint& init_point, PathData* const path_data) { - const auto frenet_point = reference_line.GetFrenetPoint(init_point); + const auto frenet_point = + reference_line.GetFrenetPoint(init_point.path_point()); const auto& qp_config = config_.qp_piecewise_jerk_path_config(); const auto& adc_sl = reference_line_info_->AdcSlBoundary(); const double qp_delta_s = qp_config.qp_delta_s(); diff --git a/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc b/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc index 3da5a339ee52149b75bdb03aaacce344c3f51695..fd07a8dad2a276070b32697b2ad709072d877450 100644 --- a/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc +++ b/modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc @@ -92,7 +92,7 @@ bool QpSplinePathGenerator::Generate( last_discretized_path_ = &path_data_history.back().first; } - init_frenet_point_ = reference_line_.GetFrenetPoint(init_point); + init_frenet_point_ = reference_line_.GetFrenetPoint(init_point.path_point()); if (is_change_lane_path_) { ref_l_ = init_frenet_point_.l(); diff --git a/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc b/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc index 7fb0d53f421b76acc84d87533ff057e4d1e2f0a4..6595b7d6cd28e2ddb0f2d66c60b313531f85f205 100644 --- a/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc +++ b/modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc @@ -69,7 +69,8 @@ bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point, << init_point.DebugString(); return false; } - init_frenet_frame_point_ = reference_line_.GetFrenetPoint(init_point_); + init_frenet_frame_point_ = + reference_line_.GetFrenetPoint(init_point_.path_point()); waypoint_sampler_->Init(&reference_line_info_, init_sl_point_, init_frenet_frame_point_);