From 94b4265ea008a20d17c969f874aae7acfd7716e7 Mon Sep 17 00:00:00 2001 From: Yajia Zhang Date: Thu, 14 Feb 2019 18:59:50 -0800 Subject: [PATCH] planning: refactored interface for Frame --- modules/planning/common/ego_info_test.cc | 3 +-- modules/planning/common/frame.cc | 14 +++----------- modules/planning/common/frame.h | 12 ++---------- .../navi/decider/navi_path_decider_test.cc | 2 +- modules/planning/navi_planning.cc | 13 ++++++------- modules/planning/navi_planning.h | 4 +--- modules/planning/on_lane_planning.cc | 13 ++++++------- modules/planning/on_lane_planning.h | 3 +-- modules/planning/open_space_planning.cc | 7 +++---- modules/planning/open_space_planning.h | 3 +-- .../planner/lattice/lattice_planner.cc | 3 ++- .../planning/planner/lattice/lattice_planner.h | 3 ++- modules/planning/planner/navi/navi_planner.cc | 3 ++- modules/planning/planner/navi/navi_planner.h | 3 ++- .../planner/open_space/open_space_planner.h | 3 ++- modules/planning/planner/planner.h | 3 ++- .../planner/public_road/public_road_planner.cc | 18 ++++++++---------- .../planner/public_road/public_road_planner.h | 3 ++- .../planning/planner/rtk/rtk_replay_planner.cc | 3 ++- .../planning/planner/rtk/rtk_replay_planner.h | 3 ++- .../autotuning_raw_feature_generator_test.cc | 3 +-- 21 files changed, 52 insertions(+), 70 deletions(-) diff --git a/modules/planning/common/ego_info_test.cc b/modules/planning/common/ego_info_test.cc index 4b7ed0e220c..3d4ac3d40cc 100644 --- a/modules/planning/common/ego_info_test.cc +++ b/modules/planning/common/ego_info_test.cc @@ -45,11 +45,10 @@ TEST(EgoInfoTest, EgoInfoSimpleTest) { common::TrajectoryPoint planning_start_point; common::VehicleState vehicle_state; ReferenceLineProvider reference_line_provider; - ADCTrajectory trajectory; LocalView dummy_local_view; Frame frame(sequence_num, dummy_local_view, planning_start_point, - vehicle_state, &reference_line_provider, &trajectory); + vehicle_state, &reference_line_provider); ego_info->CalculateFrontObstacleClearDistance(frame.obstacles()); } diff --git a/modules/planning/common/frame.cc b/modules/planning/common/frame.cc index ba2158f66cb..b5f2ee41642 100644 --- a/modules/planning/common/frame.cc +++ b/modules/planning/common/frame.cc @@ -57,22 +57,19 @@ Frame::Frame(uint32_t sequence_num) Frame::Frame(uint32_t sequence_num, const LocalView &local_view, const common::TrajectoryPoint &planning_start_point, const common::VehicleState &vehicle_state, - ReferenceLineProvider *reference_line_provider, - ADCTrajectory *output_trajectory) + ReferenceLineProvider *reference_line_provider) : sequence_num_(sequence_num), local_view_(local_view), planning_start_point_(planning_start_point), vehicle_state_(vehicle_state), - output_trajectory_(output_trajectory), reference_line_provider_(reference_line_provider), monitor_logger_buffer_(common::monitor::MonitorMessageItem::PLANNING) {} Frame::Frame(uint32_t sequence_num, const LocalView &local_view, const common::TrajectoryPoint &planning_start_point, - const common::VehicleState &vehicle_state, - ADCTrajectory *output_trajectory) + const common::VehicleState &vehicle_state) : Frame(sequence_num, local_view, planning_start_point, - vehicle_state, nullptr, output_trajectory) {} + vehicle_state, nullptr) {} const common::TrajectoryPoint &Frame::PlanningStartPoint() const { return planning_start_point_; @@ -363,11 +360,6 @@ Status Frame::InitFrameData() { ADEBUG << "Enabled align prediction time ? : " << std::boolalpha << FLAGS_align_prediction_time; - // if (FLAGS_enable_lag_prediction && lag_predictor_) { - // lag_predictor_->GetLaggedPrediction( - // local_view_.prediction_obstacles.get()); - //} - if (FLAGS_align_prediction_time) { auto prediction = *(local_view_.prediction_obstacles); AlignPredictionTime(vehicle_state_.timestamp(), &prediction); diff --git a/modules/planning/common/frame.h b/modules/planning/common/frame.h index 5ce25e017bb..9682f236118 100644 --- a/modules/planning/common/frame.h +++ b/modules/planning/common/frame.h @@ -61,13 +61,11 @@ class Frame { explicit Frame(uint32_t sequence_num, const LocalView &local_view, const common::TrajectoryPoint &planning_start_point, const common::VehicleState &vehicle_state, - ReferenceLineProvider *reference_line_provider, - ADCTrajectory *output_trajectory); + ReferenceLineProvider *reference_line_provider); explicit Frame(uint32_t sequence_num, const LocalView &local_view, const common::TrajectoryPoint &planning_start_point, - const common::VehicleState &vehicle_state, - ADCTrajectory *output_trajectory); + const common::VehicleState &vehicle_state); const common::TrajectoryPoint &PlanningStartPoint() const; @@ -124,8 +122,6 @@ class Frame { const ADCTrajectory &trajectory() const { return trajectory_; } - ADCTrajectory *output_trajectory() { return output_trajectory_; } - planning_internal::OpenSpaceDebug *mutable_open_space_debug() { return &open_space_debug_; } @@ -201,10 +197,6 @@ class Frame { // stitching trajectory for open space planner std::vector stitching_trajectory_; - // TODO(all): change to use shared_ptr. - // output trajectory pb - ADCTrajectory *output_trajectory_ = nullptr; // not owned - const ReferenceLineProvider *reference_line_provider_ = nullptr; std::vector future_route_waypoints_; diff --git a/modules/planning/navi/decider/navi_path_decider_test.cc b/modules/planning/navi/decider/navi_path_decider_test.cc index c1185b65792..16be6e4428f 100644 --- a/modules/planning/navi/decider/navi_path_decider_test.cc +++ b/modules/planning/navi/decider/navi_path_decider_test.cc @@ -162,7 +162,7 @@ TEST_F(NaviPathDeciderTest, KeepLane) { vehicle_state, plan_start_point, ref_line, route_segments); LocalView local_view; navi_path_decider.frame_ = new Frame(1, local_view, plan_start_point, - vehicle_state, nullptr, nullptr); + vehicle_state, nullptr); DCHECK_NOTNULL(navi_path_decider.reference_line_info_); DCHECK_NOTNULL(navi_path_decider.frame_); GeneratePathData(kMaxS, 0.19, 0.03, &path_points); diff --git a/modules/planning/navi_planning.cc b/modules/planning/navi_planning.cc index 24688659356..ea8f0c99a58 100644 --- a/modules/planning/navi_planning.cc +++ b/modules/planning/navi_planning.cc @@ -89,12 +89,10 @@ Status NaviPlanning::Init(const PlanningConfig& config) { Status NaviPlanning::InitFrame(const uint32_t sequence_num, const TrajectoryPoint& planning_start_point, - const double start_time, - const VehicleState& vehicle_state, - ADCTrajectory* output_trajectory) { + const VehicleState& vehicle_state) { frame_.reset(new Frame(sequence_num, local_view_, planning_start_point, vehicle_state, - reference_line_provider_.get(), output_trajectory)); + reference_line_provider_.get())); std::list reference_lines; std::list segments; @@ -195,8 +193,8 @@ void NaviPlanning::RunOnce(const LocalView& local_view, last_publishable_trajectory_.get(), &replan_reason); const uint32_t frame_num = static_cast(seq_num_++); - status = InitFrame(frame_num, stitching_trajectory.back(), start_timestamp, - vehicle_state, trajectory_pb); + status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state); + if (!frame_) { std::string msg("Failed to init frame"); AERROR << msg; @@ -474,7 +472,8 @@ Status NaviPlanning::Plan( stitching_trajectory.back()); } - auto status = planner_->Plan(stitching_trajectory.back(), frame_.get()); + auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(), + trajectory_pb); ExportReferenceLineDebug(ptr_debug); diff --git a/modules/planning/navi_planning.h b/modules/planning/navi_planning.h index f6e637da89f..2e7f059ea7e 100644 --- a/modules/planning/navi_planning.h +++ b/modules/planning/navi_planning.h @@ -75,9 +75,7 @@ class NaviPlanning : public PlanningBase { private: common::Status InitFrame(const uint32_t sequence_num, const common::TrajectoryPoint& planning_start_point, - const double start_time, - const common::VehicleState& vehicle_state, - ADCTrajectory* output_trajectory); + const common::VehicleState& vehicle_state); bool CheckPlanningConfig(const PlanningConfig& config); diff --git a/modules/planning/on_lane_planning.cc b/modules/planning/on_lane_planning.cc index 3ab8157d54f..5a235af654f 100644 --- a/modules/planning/on_lane_planning.cc +++ b/modules/planning/on_lane_planning.cc @@ -103,11 +103,10 @@ Status OnLanePlanning::Init(const PlanningConfig& config) { Status OnLanePlanning::InitFrame(const uint32_t sequence_num, const TrajectoryPoint& planning_start_point, - const VehicleState& vehicle_state, - ADCTrajectory* output_trajectory) { + const VehicleState& vehicle_state) { frame_.reset(new Frame(sequence_num, local_view_, planning_start_point, - vehicle_state, reference_line_provider_.get(), - output_trajectory)); + vehicle_state, reference_line_provider_.get())); + if (frame_ == nullptr) { return Status(ErrorCode::PLANNING_ERROR, "Fail to init frame: nullptr."); } @@ -241,8 +240,7 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, const uint32_t frame_num = static_cast(seq_num_++); bool update_ego_info = EgoInfo::Instance()->Update(stitching_trajectory.back(), vehicle_state); - status = InitFrame(frame_num, stitching_trajectory.back(), - vehicle_state, trajectory_pb); + status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state); if (update_ego_info && status.ok()) { EgoInfo::Instance()->CalculateFrontObstacleClearDistance( @@ -372,7 +370,8 @@ Status OnLanePlanning::Plan( stitching_trajectory.back()); } - auto status = planner_->Plan(stitching_trajectory.back(), frame_.get()); + auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(), + trajectory_pb); ptr_debug->mutable_planning_data()->set_front_clear_distance( EgoInfo::Instance()->front_clear_distance()); diff --git a/modules/planning/on_lane_planning.h b/modules/planning/on_lane_planning.h index 68b8293a04d..585eb1ac7b9 100644 --- a/modules/planning/on_lane_planning.h +++ b/modules/planning/on_lane_planning.h @@ -71,8 +71,7 @@ class OnLanePlanning : public PlanningBase { private: common::Status InitFrame(const uint32_t sequence_num, const common::TrajectoryPoint& planning_start_point, - const common::VehicleState& vehicle_state, - ADCTrajectory* output_trajectory); + const common::VehicleState& vehicle_state); void ExportReferenceLineDebug(planning_internal::Debug* debug); bool CheckPlanningConfig(const PlanningConfig& config); diff --git a/modules/planning/open_space_planning.cc b/modules/planning/open_space_planning.cc index 3306337f951..2e887ede6db 100644 --- a/modules/planning/open_space_planning.cc +++ b/modules/planning/open_space_planning.cc @@ -91,10 +91,9 @@ Status OpenSpacePlanning::Init(const PlanningConfig& config) { Status OpenSpacePlanning::InitFrame(const uint32_t sequence_num, const TrajectoryPoint& planning_start_point, - const VehicleState& vehicle_state, - ADCTrajectory* output_trajectory) { + const VehicleState& vehicle_state) { frame_.reset(new Frame(sequence_num, local_view_, planning_start_point, - vehicle_state, output_trajectory)); + vehicle_state)); if (frame_ == nullptr) { return Status(ErrorCode::PLANNING_ERROR, "Fail to init frame: nullptr."); } @@ -166,7 +165,7 @@ void OpenSpacePlanning::RunOnce(const LocalView& local_view, const size_t frame_num = seq_num_++; status = InitFrame(static_cast(frame_num), stitching_trajectory.back(), - vehicle_state, ptr_trajectory_pb); + vehicle_state); ptr_trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms( Clock::NowInSeconds() - start_timestamp); diff --git a/modules/planning/open_space_planning.h b/modules/planning/open_space_planning.h index b449d660456..51e1e3cbbea 100644 --- a/modules/planning/open_space_planning.h +++ b/modules/planning/open_space_planning.h @@ -88,8 +88,7 @@ class OpenSpacePlanning : public PlanningBase { apollo::common::Status InitFrame( const uint32_t sequence_num, const common::TrajectoryPoint& planning_start_point, - const common::VehicleState& vehicle_state, - ADCTrajectory* output_trajectory); + const common::VehicleState& vehicle_state); bool CheckPlanningConfig(const PlanningConfig& config); diff --git a/modules/planning/planner/lattice/lattice_planner.cc b/modules/planning/planner/lattice/lattice_planner.cc index c1246004b24..d521fb0bd06 100644 --- a/modules/planning/planner/lattice/lattice_planner.cc +++ b/modules/planning/planner/lattice/lattice_planner.cc @@ -93,7 +93,8 @@ void ComputeInitFrenetState(const PathPoint& matched_point, } // namespace Status LatticePlanner::Plan(const TrajectoryPoint& planning_start_point, - Frame* frame) { + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) { size_t success_line_count = 0; size_t index = 0; for (auto& reference_line_info : *frame->mutable_reference_line_info()) { diff --git a/modules/planning/planner/lattice/lattice_planner.h b/modules/planning/planner/lattice/lattice_planner.h index 09920540e72..e548217f509 100644 --- a/modules/planning/planner/lattice/lattice_planner.h +++ b/modules/planning/planner/lattice/lattice_planner.h @@ -52,7 +52,8 @@ class LatticePlanner : public PlannerWithReferenceLine { * @return OK if planning succeeds; error otherwise. */ common::Status Plan(const common::TrajectoryPoint& planning_init_point, - Frame* frame) override; + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) override; /** * @brief Override function Plan in parent class Planner. diff --git a/modules/planning/planner/navi/navi_planner.cc b/modules/planning/planner/navi/navi_planner.cc index 36cbd8d8dab..edf230d5b2d 100644 --- a/modules/planning/planner/navi/navi_planner.cc +++ b/modules/planning/planner/navi/navi_planner.cc @@ -98,7 +98,8 @@ Status NaviPlanner::Init(const PlanningConfig& config) { } Status NaviPlanner::Plan(const TrajectoryPoint& planning_init_point, - Frame* frame) { + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) { // NaviPlanner is only used in navigation mode based on the real-time relative // map. if (!FLAGS_use_navigation_mode) { diff --git a/modules/planning/planner/navi/navi_planner.h b/modules/planning/planner/navi/navi_planner.h index 97b1e9616ca..280a664bbad 100644 --- a/modules/planning/planner/navi/navi_planner.h +++ b/modules/planning/planner/navi/navi_planner.h @@ -71,7 +71,8 @@ class NaviPlanner : public PlannerWithReferenceLine { * @return OK if planning succeeds; error otherwise. */ common::Status Plan(const common::TrajectoryPoint& planning_init_point, - Frame* frame) override; + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) override; void Stop() override {} diff --git a/modules/planning/planner/open_space/open_space_planner.h b/modules/planning/planner/open_space/open_space_planner.h index 95345e78447..16cc4df4188 100644 --- a/modules/planning/planner/open_space/open_space_planner.h +++ b/modules/planning/planner/open_space/open_space_planner.h @@ -86,7 +86,8 @@ class OpenSpacePlanner : public Planner { */ apollo::common::Status Plan( const common::TrajectoryPoint& planning_init_point, - Frame* frame) override { + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) override { return Status::OK(); } diff --git a/modules/planning/planner/planner.h b/modules/planning/planner/planner.h index eb4d6573a85..c5870af9900 100644 --- a/modules/planning/planner/planner.h +++ b/modules/planning/planner/planner.h @@ -61,7 +61,8 @@ class Planner { * @return OK if planning succeeds; error otherwise. */ virtual apollo::common::Status Plan( - const common::TrajectoryPoint& planning_init_point, Frame* frame) = 0; + const common::TrajectoryPoint& planning_init_point, Frame* frame, + ADCTrajectory* ptr_computed_trajectory) = 0; virtual void Stop() = 0; diff --git a/modules/planning/planner/public_road/public_road_planner.cc b/modules/planning/planner/public_road/public_road_planner.cc index d30bdc6c6c2..fa8395935b5 100644 --- a/modules/planning/planner/public_road/public_road_planner.cc +++ b/modules/planning/planner/public_road/public_road_planner.cc @@ -43,22 +43,20 @@ Status PublicRoadPlanner::Init(const PlanningConfig& config) { } Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point, - Frame* frame) { + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) { DCHECK_NOTNULL(frame); scenario_manager_.Update(planning_start_point, *frame); scenario_ = scenario_manager_.mutable_scenario(); auto result = scenario_->Process(planning_start_point, frame); if (FLAGS_enable_record_debug) { - if (frame->output_trajectory()) { - auto scenario_debug = frame->output_trajectory() - ->mutable_debug() - ->mutable_planning_data() - ->mutable_scenario(); - scenario_debug->set_scenario_type(scenario_->scenario_type()); - scenario_debug->set_stage_type(scenario_->GetStage()); - scenario_debug->set_msg(scenario_->GetMsg()); - } + auto scenario_debug = + ptr_computed_trajectory->mutable_debug() + ->mutable_planning_data()->mutable_scenario(); + scenario_debug->set_scenario_type(scenario_->scenario_type()); + scenario_debug->set_stage_type(scenario_->GetStage()); + scenario_debug->set_msg(scenario_->GetMsg()); } if (result == scenario::Scenario::STATUS_DONE) { diff --git a/modules/planning/planner/public_road/public_road_planner.h b/modules/planning/planner/public_road/public_road_planner.h index f79caa7a899..cdadf3dab91 100644 --- a/modules/planning/planner/public_road/public_road_planner.h +++ b/modules/planning/planner/public_road/public_road_planner.h @@ -69,7 +69,8 @@ class PublicRoadPlanner : public PlannerWithReferenceLine { */ apollo::common::Status Plan( const common::TrajectoryPoint& planning_init_point, - Frame* frame) override; + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) override; }; } // namespace planning diff --git a/modules/planning/planner/rtk/rtk_replay_planner.cc b/modules/planning/planner/rtk/rtk_replay_planner.cc index 682f71b3921..5dd482048fe 100644 --- a/modules/planning/planner/rtk/rtk_replay_planner.cc +++ b/modules/planning/planner/rtk/rtk_replay_planner.cc @@ -37,7 +37,8 @@ RTKReplayPlanner::RTKReplayPlanner() { Status RTKReplayPlanner::Init(const PlanningConfig&) { return Status::OK(); } Status RTKReplayPlanner::Plan(const TrajectoryPoint& planning_start_point, - Frame* frame) { + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) { auto status = Status::OK(); bool has_plan = false; auto it = std::find_if( diff --git a/modules/planning/planner/rtk/rtk_replay_planner.h b/modules/planning/planner/rtk/rtk_replay_planner.h index bf9e4b13896..06f845c62b5 100644 --- a/modules/planning/planner/rtk/rtk_replay_planner.h +++ b/modules/planning/planner/rtk/rtk_replay_planner.h @@ -64,7 +64,8 @@ class RTKReplayPlanner : public PlannerWithReferenceLine { */ apollo::common::Status Plan( const common::TrajectoryPoint& planning_init_point, - Frame* frame) override; + Frame* frame, + ADCTrajectory* ptr_computed_trajectory) override; /** * @brief Override function Plan in parent class Planner. diff --git a/modules/planning/tuning/autotuning_raw_feature_generator_test.cc b/modules/planning/tuning/autotuning_raw_feature_generator_test.cc index 8e5ab32b5b6..73c8046f167 100644 --- a/modules/planning/tuning/autotuning_raw_feature_generator_test.cc +++ b/modules/planning/tuning/autotuning_raw_feature_generator_test.cc @@ -38,8 +38,7 @@ class AutotuningRawFeatureGeneratorTest : public ::testing::Test { new ReferenceLineInfo(ego_state, ego_pos, reference_line, segments)); // pseudo empty frame info LocalView dummy_local_view; - frame_.reset(new Frame(0, dummy_local_view, ego_pos, ego_state, nullptr, - nullptr)); + frame_.reset(new Frame(0, dummy_local_view, ego_pos, ego_state, nullptr)); speed_limit_.reset(new SpeedLimit()); generator_.reset(new AutotuningRawFeatureGenerator(8, 17, *ref_line_info_, *frame_, *speed_limit_));