diff --git a/modules/planning/planner/em/em_planner.cc b/modules/planning/planner/em/em_planner.cc index 2b56f0d94796883b91605749932c9d89c8d1a6fa..4554f04f484cea4f6452aa329240afa9c5c0307f 100644 --- a/modules/planning/planner/em/em_planner.cc +++ b/modules/planning/planner/em/em_planner.cc @@ -102,14 +102,14 @@ void EMPlanner::RecordDebugInfo(const std::string& name, em_planner_debugger_.speed_profiles_[name].second = time_diff_ms; } -Status EMPlanner::Plan(Frame* frame, +Status EMPlanner::Plan(const TrajectoryPoint& planning_start_point, + Frame* frame, PublishableTrajectory* ptr_publishable_trajectory) { if (!frame) { AERROR << "Frame is empty in EMPlanner"; return Status(ErrorCode::PLANNING_ERROR, "Frame is null"); } - const auto& planning_start_point = frame->PlanningStartPoint(); ADEBUG << "planning start point:" << planning_start_point.DebugString(); auto* planning_data = frame->mutable_planning_data(); diff --git a/modules/planning/planner/em/em_planner.h b/modules/planning/planner/em/em_planner.h index 91f5c0b37cd10c1a7d4078b578f07b035d2777bf..fde133e201cdccc3326d35a37fa569c5f35606b5 100644 --- a/modules/planning/planner/em/em_planner.h +++ b/modules/planning/planner/em/em_planner.h @@ -65,7 +65,8 @@ class EMPlanner : public Planner { * @param trajectory_pb The computed trajectory * @return OK if planning succeeds; error otherwise. */ - apollo::common::Status Plan(Frame* frame, + apollo::common::Status Plan(const TrajectoryPoint& planning_init_point, + Frame* frame, PublishableTrajectory* trajectory_pb) override; EMPlannerDebugger& em_planner_debugger(); diff --git a/modules/planning/planner/planner.h b/modules/planning/planner/planner.h index d5cd26b2e41719dd4d4010d2b9ea84a4fd0f6330..003376cc35af279b747efaa68db07a7ae9d47a1c 100644 --- a/modules/planning/planner/planner.h +++ b/modules/planning/planner/planner.h @@ -63,7 +63,8 @@ class Planner { * @param trajectory_pb The computed trajectory * @return OK if planning succeeds; error otherwise. */ - virtual apollo::common::Status Plan(Frame* frame, + virtual apollo::common::Status Plan(const TrajectoryPoint& planning_init_point, + Frame* frame, PublishableTrajectory* trajectory_pb) = 0; }; diff --git a/modules/planning/planner/rtk/rtk_replay_planner.cc b/modules/planning/planner/rtk/rtk_replay_planner.cc index 76102dbefb4e33a8dc257434142bfe397cb06525..282d35bc39c0a984a8e0b91049228d5a21633fe0 100644 --- a/modules/planning/planner/rtk/rtk_replay_planner.cc +++ b/modules/planning/planner/rtk/rtk_replay_planner.cc @@ -36,7 +36,7 @@ RTKReplayPlanner::RTKReplayPlanner() { Status RTKReplayPlanner::Init(const PlanningConfig&) { return Status::OK(); } -Status RTKReplayPlanner::Plan( +Status RTKReplayPlanner::Plan(const TrajectoryPoint& planning_init_point, Frame* frame, PublishableTrajectory* ptr_publishable_trajectory) { if (complete_rtk_trajectory_.empty() || complete_rtk_trajectory_.size() < 2) { std::string msg( @@ -46,10 +46,9 @@ Status RTKReplayPlanner::Plan( AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } - const TrajectoryPoint& start_point = frame->PlanningStartPoint(); std::uint32_t matched_index = - QueryPositionMatchedPoint(start_point, complete_rtk_trajectory_); + QueryPositionMatchedPoint(planning_init_point, complete_rtk_trajectory_); std::uint32_t forward_buffer = FLAGS_rtk_trajectory_forward; std::uint32_t end_index = diff --git a/modules/planning/planner/rtk/rtk_replay_planner.h b/modules/planning/planner/rtk/rtk_replay_planner.h index 6490bc0e841d8658bdce61dd33206df4776b7281..33133533505db0599eed292a1bcc02e1279df5df 100644 --- a/modules/planning/planner/rtk/rtk_replay_planner.h +++ b/modules/planning/planner/rtk/rtk_replay_planner.h @@ -58,7 +58,8 @@ class RTKReplayPlanner : public Planner { * @param trajectory_pb The computed trajectory * @return OK if planning succeeds; error otherwise. */ - apollo::common::Status Plan(Frame* frame, + apollo::common::Status Plan(const TrajectoryPoint& planning_init_point, + Frame* frame, PublishableTrajectory* trajectory_pb) override; /** diff --git a/modules/planning/planner/rtk/rtk_replay_planner_test.cc b/modules/planning/planner/rtk/rtk_replay_planner_test.cc index 9c6507a5770f313b2095a487b9eccace9fd58377..794089e6a643771e210b7d3fb1d89e55049e7ed7 100644 --- a/modules/planning/planner/rtk/rtk_replay_planner_test.cc +++ b/modules/planning/planner/rtk/rtk_replay_planner_test.cc @@ -37,9 +37,7 @@ TEST_F(RTKReplayPlannerTest, ComputeTrajectory) { start_point.mutable_path_point()->set_y(4140674.76063); PublishableTrajectory trajectory; - Frame frame(1); - frame.SetPlanningStartPoint(start_point); - auto status = planner.Plan(&frame, &trajectory); + auto status = planner.Plan(start_point, nullptr, &trajectory); EXPECT_TRUE(status.ok()); EXPECT_TRUE(!trajectory.trajectory_points().empty()); @@ -65,9 +63,7 @@ TEST_F(RTKReplayPlannerTest, ErrorTest) { start_point.mutable_path_point()->set_x(586385.782842); start_point.mutable_path_point()->set_y(4140674.76063); PublishableTrajectory trajectory; - Frame frame(1); - frame.SetPlanningStartPoint(start_point); - EXPECT_TRUE(!(planner_with_error_csv.Plan(&frame, &trajectory)).ok()); + EXPECT_TRUE(!(planner_with_error_csv.Plan(start_point, nullptr, &trajectory)).ok()); } } // namespace planning diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc index a438f649d65bb391a6fe8073ce4bbc2d23426dc7..5c1117890da4fe529d4760409df5a5a5469b6341 100644 --- a/modules/planning/planning.cc +++ b/modules/planning/planning.cc @@ -247,7 +247,8 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time, frame_->SetPlanningStartPoint(stitching_trajectory.back()); PublishableTrajectory publishable_trajectory; - auto status = planner_->Plan(frame_.get(), &publishable_trajectory); + auto status = planner_->Plan(stitching_trajectory.back(), + frame_.get(), &publishable_trajectory); if (FLAGS_enable_record_debug) { trajectory_pb->mutable_debug() diff --git a/modules/planning/planning.h b/modules/planning/planning.h index 434026a0a87c6cfe7fe3523118ad8082ace818ce..457b72ff7e710645a0b00a12e782561889902839 100644 --- a/modules/planning/planning.h +++ b/modules/planning/planning.h @@ -76,7 +76,8 @@ class Planning : public apollo::common::ApolloApp { * @param is_on_auto_mode whether the current system is on auto-driving mode * @param trajectory_pb the computed planning trajectory */ - bool Plan(const bool is_on_auto_mode, const double publish_time, + bool Plan(const bool is_on_auto_mode, + const double publish_time, ADCTrajectory* trajectory_pb); const Frame* GetFrame() const;