From 5bc1d2ace06d92d9b64c9dca8500987a09402529 Mon Sep 17 00:00:00 2001 From: Zhang Liangliang Date: Thu, 31 Aug 2017 13:42:09 -0700 Subject: [PATCH] Planning: use init planning point on last planning trajectory. --- modules/planning/common/frame.cc | 8 +++--- .../planning/common/reference_line_info.cc | 27 ++++++++++++++----- modules/planning/common/reference_line_info.h | 2 ++ .../planner/rtk/rtk_replay_planner_test.cc | 4 +-- 4 files changed, 29 insertions(+), 12 deletions(-) diff --git a/modules/planning/common/frame.cc b/modules/planning/common/frame.cc index 2d484d360e..85ef2451af 100644 --- a/modules/planning/common/frame.cc +++ b/modules/planning/common/frame.cc @@ -94,8 +94,8 @@ bool Frame::InitReferenceLineInfo( const std::vector &reference_lines) { reference_line_info_.clear(); for (const auto &reference_line : reference_lines) { - reference_line_info_.emplace_back(pnc_map_, reference_line, - smoother_config_); + reference_line_info_.emplace_back(planning_start_point_, pnc_map_, + reference_line, smoother_config_); if (!reference_line_info_.back().Init()) { AERROR << "Failed to init reference line info"; return false; @@ -122,8 +122,8 @@ bool Frame::Init(const PlanningConfig &config, return false; } smoother_config_ = config.reference_line_smoother_config(); - auto reference_lines = CreateReferenceLineFromRouting(init_pose_.position(), - routing_response_); + auto reference_lines = + CreateReferenceLineFromRouting(init_pose_.position(), routing_response_); if (reference_lines.empty()) { AERROR << "Failed to create reference line from position: " << init_pose_.DebugString(); diff --git a/modules/planning/common/reference_line_info.cc b/modules/planning/common/reference_line_info.cc index 3ae6224ae4..ec9269b9ec 100644 --- a/modules/planning/common/reference_line_info.cc +++ b/modules/planning/common/reference_line_info.cc @@ -34,10 +34,16 @@ namespace apollo { namespace planning { +using TrajectoryPoint = apollo::common::TrajectoryPoint; +using Vec2d = apollo::common::math::Vec2d; +using SLPoint = apollo::common::SLPoint; + ReferenceLineInfo::ReferenceLineInfo( - const hdmap::PncMap* pnc_map, const ReferenceLine& reference_line, + const TrajectoryPoint& init_adc_point, const hdmap::PncMap* pnc_map, + const ReferenceLine& reference_line, const ReferenceLineSmootherConfig& smoother_config) - : pnc_map_(pnc_map), + : init_adc_point_(init_adc_point), + pnc_map_(pnc_map), reference_line_(reference_line), smoother_config_(smoother_config) {} @@ -58,14 +64,23 @@ bool ReferenceLineInfo::Init() { } bool ReferenceLineInfo::CalculateAdcSmoothReferenLinePoint() { + SLPoint adc_sl_on_raw_ref; + if (!reference_line_.XYToSL(Vec2d(init_adc_point_.path_point().x(), + init_adc_point_.path_point().y()), + &adc_sl_on_raw_ref)) { + AERROR << "Fail to get sl point for init_adc_point. init_adc_point: " + << init_adc_point_.DebugString(); + return false; + } + const double backward_s = -5.0; const double forward_s = 10.0; const double delta_s = 2.0; - double s = backward_s + adc_sl_boundary_.start_s(); + double s = backward_s + adc_sl_on_raw_ref.s(); std::vector local_ref_points; - while (s <= forward_s + adc_sl_boundary_.end_s()) { + while (s <= forward_s + adc_sl_on_raw_ref.s()) { local_ref_points.push_back(reference_line_.GetReferencePoint(s)); s += delta_s; } @@ -78,8 +93,8 @@ bool ReferenceLineInfo::CalculateAdcSmoothReferenLinePoint() { AERROR << "Failed to smooth reference line"; return false; } - adc_smooth_ref_point_ = - smoothed_segment.GetReferencePoint(adc_sl_boundary_.end_s()); + adc_smooth_ref_point_ = smoothed_segment.GetReferencePoint( + init_adc_point_.path_point().x(), init_adc_point_.path_point().y()); return true; } diff --git a/modules/planning/common/reference_line_info.h b/modules/planning/common/reference_line_info.h index 6942bb0283..0ec360802b 100644 --- a/modules/planning/common/reference_line_info.h +++ b/modules/planning/common/reference_line_info.h @@ -43,6 +43,7 @@ namespace planning { class ReferenceLineInfo { public: explicit ReferenceLineInfo( + const common::TrajectoryPoint& init_adc_point, const hdmap::PncMap* pnc_map, const ReferenceLine& reference_line, const ReferenceLineSmootherConfig& smoother_config); @@ -112,6 +113,7 @@ class ReferenceLineInfo { bool InitPerceptionSLBoundary(PathObstacle* path_obstacle); bool CalculateAdcSmoothReferenLinePoint(); + const common::TrajectoryPoint& init_adc_point_; const hdmap::PncMap* pnc_map_ = nullptr; /** diff --git a/modules/planning/planner/rtk/rtk_replay_planner_test.cc b/modules/planning/planner/rtk/rtk_replay_planner_test.cc index 752510ba51..165b98fb30 100644 --- a/modules/planning/planner/rtk/rtk_replay_planner_test.cc +++ b/modules/planning/planner/rtk/rtk_replay_planner_test.cc @@ -52,7 +52,7 @@ TEST_F(RTKReplayPlannerTest, ComputeTrajectory) { localization.mutable_pose()->mutable_linear_acceleration()->set_z(0.0); common::VehicleState::instance()->Update(localization, chassis); ReferenceLineSmootherConfig smooth_config; - ReferenceLineInfo info(nullptr, reference_line, smooth_config); + ReferenceLineInfo info(start_point, nullptr, reference_line, smooth_config); auto status = planner.Plan(start_point, nullptr, &info); const auto& trajectory = info.trajectory(); @@ -93,7 +93,7 @@ TEST_F(RTKReplayPlannerTest, ErrorTest) { common::VehicleState::instance()->Update(localization, chassis); ReferenceLine ref; ReferenceLineSmootherConfig smooth_config; - ReferenceLineInfo info(nullptr, ref, smooth_config); + ReferenceLineInfo info(start_point, nullptr, ref, smooth_config); EXPECT_TRUE(!(planner_with_error_csv.Plan(start_point, nullptr, &info)).ok()); } -- GitLab