Populate trajectory points directly in planner.

modules/planning/common/trajectory/publishable_trajectory.cc

@@ -50,12 +50,17 @@ void PublishableTrajectory::set_header_time(const double header_time) {
 ADCTrajectory PublishableTrajectory::to_trajectory_protobuf() const {
   ADCTrajectory trajectory_pb;
   trajectory_pb.mutable_header()->set_timestamp_sec(_header_time);
-  for (const auto& tp : _trajectory_points) {
-    auto* trajectory_point = trajectory_pb.add_trajectory_point();
-    trajectory_point->CopyFrom(tp);
-  }
+  trajectory_pb.mutable_trajectory_point()->MergeFrom(
+      {_trajectory_points.begin(), _trajectory_points.end()});
   return trajectory_pb;
 }
 
+void PublishableTrajectory::populate_trajectory_protobuf(
+    ADCTrajectory* trajectory_pb) const {
+  trajectory_pb->mutable_header()->set_timestamp_sec(_header_time);
+  trajectory_pb->mutable_trajectory_point()->MergeFrom(
+      {_trajectory_points.begin(), _trajectory_points.end()});
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/trajectory/publishable_trajectory.h

@@ -44,6 +44,7 @@ public:
   void set_header_time(const double header_time);
 
   ADCTrajectory to_trajectory_protobuf() const;
+  void populate_trajectory_protobuf(ADCTrajectory* trajectory_pb) const;
 
 private:
   double _header_time;

modules/planning/planner/em/em_planner.cc

@@ -81,9 +81,8 @@ Status EMPlanner::Init(const PlanningConfig& config) {
   return Status::OK();
 }
 
-Status EMPlanner::MakePlan(
-    const TrajectoryPoint& start_point,
-    std::vector<TrajectoryPoint>* discretized_trajectory) {
+Status EMPlanner::MakePlan(const TrajectoryPoint& start_point,
+                           ADCTrajectory* trajectory_pb) {
   DataCenter* data_center = DataCenter::instance();
   Frame* frame = data_center->current_frame();
 
@@ -110,7 +109,17 @@ Status EMPlanner::MakePlan(
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
   frame->set_computed_trajectory(computed_trajectory);
-  *discretized_trajectory = computed_trajectory.trajectory_points();
+  computed_trajectory.populate_trajectory_protobuf(trajectory_pb);
+
+  // Add debug information.
+  auto* debug_reference_line =
+      trajectory_pb->mutable_debug()->mutable_planning_data()->add_path();
+  debug_reference_line->set_name("planning_reference_line");
+  const auto& reference_points =
+      planning_data->reference_line().reference_points();
+  debug_reference_line->mutable_path()->mutable_path_point()->CopyFrom({
+      reference_points.begin(), reference_points.end()});
+
   return Status::OK();
 }
 

modules/planning/planner/em/em_planner.h

@@ -61,12 +61,12 @@ class EMPlanner : public Planner {
   /**
    * @brief Overrode function Plan in parent class Planner.
    * @param start_point The trajectory point where planning starts
-   * @param discretized_trajectory The computed trajectory
-   * @return true if planning succeeds; false otherwise.
+   * @param trajectory_pb The computed trajectory
+   * @return OK if planning succeeds; error otherwise.
    */
   apollo::common::Status MakePlan(
       const apollo::common::TrajectoryPoint& start_point,
-      std::vector<apollo::common::TrajectoryPoint>* trajectory) override;
+      ADCTrajectory* trajectory_pb) override;
 
  private:
   void RegisterOptimizers();

modules/planning/planner/planner.h

@@ -20,9 +20,10 @@
 #include <vector>
 
 #include "modules/common/proto/path_point.pb.h"
-#include "modules/planning/proto/planning_config.pb.h"
 #include "modules/common/status/status.h"
 #include "modules/common/vehicle_state/vehicle_state.h"
+#include "modules/planning/proto/planning.pb.h"
+#include "modules/planning/proto/planning_config.pb.h"
 
 /**
  * @namespace apollo::planning
@@ -54,12 +55,12 @@ class Planner {
   /**
    * @brief Compute a trajectory for execution.
    * @param start_point The trajectory point where planning starts
-   * @param discretized_trajectory The computed trajectory
-   * @return true if planning succeeds; false otherwise.
+   * @param trajectory_pb The computed trajectory
+   * @return OK if planning succeeds; error otherwise.
    */
   virtual apollo::common::Status MakePlan(
       const apollo::common::TrajectoryPoint& start_point,
-      std::vector<apollo::common::TrajectoryPoint>* discretized_trajectory) = 0;
+      ADCTrajectory* trajectory_pb) = 0;
 };
 
 }  // namespace planning

modules/planning/planner/rtk/rtk_replay_planner.cc

@@ -38,9 +38,8 @@ Status RTKReplayPlanner::Init(const PlanningConfig& config) {
   return Status::OK();
 }
 
-Status RTKReplayPlanner::MakePlan(
-    const TrajectoryPoint& start_point,
-    std::vector<TrajectoryPoint>* ptr_discretized_trajectory) {
+Status RTKReplayPlanner::MakePlan(const TrajectoryPoint& start_point,
+                                  ADCTrajectory* trajectory_pb) {
   if (complete_rtk_trajectory_.empty() || complete_rtk_trajectory_.size() < 2) {
     std::string msg("RTKReplayPlanner doesn't have a recorded trajectory or "
               "the recorded trajectory doesn't have enough valid trajectory "
@@ -58,24 +57,25 @@ Status RTKReplayPlanner::MakePlan(
           ? complete_rtk_trajectory_.size() - 1
           : matched_index + forward_buffer - 1;
 
-  ptr_discretized_trajectory->assign(
-      complete_rtk_trajectory_.begin() + matched_index,
-      complete_rtk_trajectory_.begin() + end_index + 1);
+  auto* trajectory_points = trajectory_pb->mutable_trajectory_point();
+  *trajectory_points = {complete_rtk_trajectory_.begin() + matched_index,
+                        complete_rtk_trajectory_.begin() + end_index + 1};
 
   // reset relative time
   double zero_time = complete_rtk_trajectory_[matched_index].relative_time();
-  for (auto &trajectory : *ptr_discretized_trajectory) {
+  for (auto &trajectory : *trajectory_points) {
     trajectory.set_relative_time(trajectory.relative_time() - zero_time);
   }
 
   // check if the trajectory has enough points;
   // if not, append the last points multiple times and
   // adjust their corresponding time stamps.
-  while (ptr_discretized_trajectory->size() < FLAGS_rtk_trajectory_forward) {
-    ptr_discretized_trajectory->push_back(ptr_discretized_trajectory->back());
-    auto& last_point = ptr_discretized_trajectory->back();
-    last_point.set_relative_time(last_point.relative_time() +
-                                 FLAGS_trajectory_resolution);
+  while (trajectory_points->size() < (size_t)FLAGS_rtk_trajectory_forward) {
+    const auto& last_point = *trajectory_points->rbegin();
+    auto* new_point = trajectory_points->Add();
+    *new_point = last_point;
+    new_point->set_relative_time(
+        new_point->relative_time() + FLAGS_trajectory_resolution);
   }
   return Status::OK();
 }

modules/planning/planner/rtk/rtk_replay_planner.h

@@ -54,12 +54,12 @@ class RTKReplayPlanner : public Planner {
   /**
    * @brief Overrode function Plan in parent class Planner.
    * @param start_point The trajectory point where planning starts
-   * @param discretized_trajectory The computed trajectory
-   * @return true if planning succeeds; false otherwise.
+   * @param trajectory_pb The computed trajectory
+   * @return OK if planning succeeds; error otherwise.
    */
   apollo::common::Status MakePlan(
       const apollo::common::TrajectoryPoint& start_point,
-      std::vector<apollo::common::TrajectoryPoint>* ptr_trajectory) override;
+      ADCTrajectory* trajectory_pb) override;
 
   /**
    * @brief Read the recorded trajectory file.

modules/planning/planner/rtk/rtk_replay_planner_test.cc

@@ -36,20 +36,20 @@ TEST_F(RTKReplayPlannerTest, ComputeTrajectory) {
   start_point.mutable_path_point()->set_x(586385.782842);
   start_point.mutable_path_point()->set_y(4140674.76063);
 
-  std::vector<TrajectoryPoint> trajectory;
+  ADCTrajectory trajectory;
   auto status = planner.MakePlan(start_point, &trajectory);
 
   EXPECT_TRUE(status.ok());
-  EXPECT_TRUE(!trajectory.empty());
-  EXPECT_EQ(trajectory.size(), (std::uint32_t)FLAGS_rtk_trajectory_forward);
+  EXPECT_TRUE(!trajectory.trajectory_point().empty());
+  EXPECT_EQ(trajectory.trajectory_point_size(), FLAGS_rtk_trajectory_forward);
 
-  auto first_point = trajectory.front();
-  EXPECT_DOUBLE_EQ(first_point.path_point().x(), 586385.782841);
-  EXPECT_DOUBLE_EQ(first_point.path_point().y(), 4140674.76065);
+  auto first_point = trajectory.trajectory_point().begin();
+  EXPECT_DOUBLE_EQ(first_point->path_point().x(), 586385.782841);
+  EXPECT_DOUBLE_EQ(first_point->path_point().y(), 4140674.76065);
 
-  auto last_point = trajectory.back();
-  EXPECT_DOUBLE_EQ(last_point.path_point().x(), 586355.063786);
-  EXPECT_DOUBLE_EQ(last_point.path_point().y(), 4140681.98605);
+  auto last_point = trajectory.trajectory_point().rbegin();
+  EXPECT_DOUBLE_EQ(last_point->path_point().x(), 586355.063786);
+  EXPECT_DOUBLE_EQ(last_point->path_point().y(), 4140681.98605);
 }
 
 TEST_F(RTKReplayPlannerTest, ErrorTest) {
@@ -61,7 +61,7 @@ TEST_F(RTKReplayPlannerTest, ErrorTest) {
   TrajectoryPoint start_point;
   start_point.mutable_path_point()->set_x(586385.782842);
   start_point.mutable_path_point()->set_y(4140674.76063);
-  std::vector<TrajectoryPoint> trajectory;
+  ADCTrajectory trajectory;
   EXPECT_TRUE(
       !(planner_with_error_csv.MakePlan(start_point, &trajectory)).ok());
 }

modules/planning/planning.cc

@@ -17,6 +17,7 @@
 #include "modules/planning/planning.h"
 
 #include <algorithm>
+#include <google/protobuf/repeated_field.h>
 
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/time/time.h"
@@ -130,7 +131,6 @@ void Planning::RunOnce() {
   ADEBUG << "Get chassis:" << chassis.DebugString();
 
   common::VehicleState::instance()->Update(localization, chassis);
-  bool is_on_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
 
   double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
   // the execution_start_time is the estimated time when the planned trajectory
@@ -145,12 +145,13 @@ void Planning::RunOnce() {
     return;
   }
 
-  std::vector<TrajectoryPoint> planning_trajectory;
-  bool res_planning =
-      Plan(is_on_auto_mode, execution_start_time, &planning_trajectory);
+  ADCTrajectory trajectory_pb;
+  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
+  bool res_planning = Plan(is_auto_mode, execution_start_time, &trajectory_pb);
   if (res_planning) {
-    ADCTrajectory trajectory_pb =
-        ToADCTrajectory(execution_start_time, planning_trajectory);
+    AdapterManager::FillPlanningHeader("planning",
+                                       trajectory_pb.mutable_header());
+    trajectory_pb.mutable_header()->set_timestamp_sec(execution_start_time);
     AdapterManager::PublishPlanning(trajectory_pb);
     ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString();
   } else {
@@ -161,7 +162,7 @@ void Planning::RunOnce() {
 void Planning::Stop() {}
 
 bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
-                    std::vector<TrajectoryPoint>* planning_trajectory) {
+                    ADCTrajectory* trajectory_pb) {
   double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
   double execution_start_time = publish_time;
 
@@ -190,7 +191,7 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
     if (position_deviation < FLAGS_replanning_threshold) {
       // planned trajectory from the matched point, the matched point has
       // relative time 0.
-      auto status = planner_->MakePlan(matched_point, planning_trajectory);
+      auto status = planner_->MakePlan(matched_point, trajectory_pb);
 
       if (!status.ok()) {
         last_trajectory_.clear();
@@ -199,14 +200,12 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
 
       // a segment of last trajectory to be attached to planned trajectory in
       // case controller needs.
-      auto overhead_trajectory = GetOverheadTrajectory(
-          matched_index, (std::uint32_t)FLAGS_rtk_trajectory_backward);
-      planning_trajectory->insert(planning_trajectory->begin(),
-                                  overhead_trajectory.begin(),
-                                  overhead_trajectory.end());
+      GetOverheadTrajectory(matched_index, FLAGS_rtk_trajectory_backward,
+                            trajectory_pb);
 
       // store the planned trajectory and header info for next planning cycle.
-      last_trajectory_ = *planning_trajectory;
+      last_trajectory_ = {trajectory_pb->trajectory_point().begin(),
+                          trajectory_pb->trajectory_point().end()};
       last_header_time_ = execution_start_time;
       return true;
     }
@@ -219,13 +218,14 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
   TrajectoryPoint vehicle_state_point =
       ComputeStartingPointFromVehicleState(planning_cycle_time);
 
-  auto status = planner_->MakePlan(vehicle_state_point, planning_trajectory);
+  auto status = planner_->MakePlan(vehicle_state_point, trajectory_pb);
   if (!status.ok()) {
     last_trajectory_.clear();
     return false;
   }
   // store the planned trajectory and header info for next planning cycle.
-  last_trajectory_ = *planning_trajectory;
+  last_trajectory_ = {trajectory_pb->trajectory_point().begin(),
+                      trajectory_pb->trajectory_point().end()};
   last_header_time_ = execution_start_time;
   return true;
 }
@@ -278,12 +278,13 @@ void Planning::Reset() {
   last_trajectory_.clear();
 }
 
-std::vector<TrajectoryPoint> Planning::GetOverheadTrajectory(
-    const std::uint32_t matched_index, const std::uint32_t buffer_size) {
+void Planning::GetOverheadTrajectory(const std::uint32_t matched_index,
+                                     const std::uint32_t buffer_size,
+                                     ADCTrajectory* trajectory_pb) {
   const std::uint32_t start_index =
       matched_index < buffer_size ? 0 : matched_index - buffer_size;
 
-  std::vector<TrajectoryPoint> overhead_trajectory(
+  google::protobuf::RepeatedPtrField<TrajectoryPoint> overhead_trajectory(
       last_trajectory_.begin() + start_index,
       last_trajectory_.begin() + matched_index);
 
@@ -292,31 +293,10 @@ std::vector<TrajectoryPoint> Planning::GetOverheadTrajectory(
   for (auto& p : overhead_trajectory) {
     p.set_relative_time(p.relative_time() - zero_relative_time);
   }
-  return overhead_trajectory;
-}
 
-ADCTrajectory Planning::ToADCTrajectory(
-    const double header_time,
-    const std::vector<TrajectoryPoint>& discretized_trajectory) {
-  ADCTrajectory trajectory_pb;
-  AdapterManager::FillPlanningHeader("planning",
-                                     trajectory_pb.mutable_header());
-
-  trajectory_pb.mutable_header()->set_timestamp_sec(header_time);
-  trajectory_pb.mutable_trajectory_point()->MergeFrom(
-      {discretized_trajectory.begin(), discretized_trajectory.end()});
-
-  // Add debug information.
-  auto* debug_reference_line =
-      trajectory_pb.mutable_debug()->mutable_planning_data()->add_path();
-  debug_reference_line->set_name("planning_reference_line");
-  const auto& reference_points =
-      DataCenter::instance()->current_frame()->planning_data()
-          .reference_line().reference_points();
-  debug_reference_line->mutable_path()->mutable_path_point()->CopyFrom({
-      reference_points.begin(), reference_points.end()});
-
-  return std::move(trajectory_pb);
+  // Insert the overhead_trajectory to the head of trajectory_pb.
+  overhead_trajectory.MergeFrom(trajectory_pb->trajectory_point());
+  trajectory_pb->mutable_trajectory_point()->Swap(&overhead_trajectory);
 }
 
 }  // namespace planning

modules/planning/planning.h

@@ -73,10 +73,10 @@ class Planning : public apollo::common::ApolloApp {
   /**
    * @brief Plan the trajectory given current vehicle state
    * @param is_on_auto_mode whether the current system is on auto-driving mode
-   * @param publishable_trajectory the computed planning trajectory
+   * @param trajectory_pb the computed planning trajectory
    */
   bool Plan(const bool is_on_auto_mode, const double publish_time,
-            std::vector<common::TrajectoryPoint> *discretized_trajectory);
+            ADCTrajectory* trajectory_pb);
 
   /**
    * @brief Reset the planner to initial state.
@@ -93,12 +93,9 @@ class Planning : public apollo::common::ApolloApp {
   common::TrajectoryPoint ComputeStartingPointFromVehicleState(
       const double forward_time) const;
 
-  std::vector<common::TrajectoryPoint> GetOverheadTrajectory(
-      const std::uint32_t matched_index, const std::uint32_t buffer_size);
-
-  ADCTrajectory ToADCTrajectory(
-      const double header_time,
-      const std::vector<common::TrajectoryPoint> &discretized_trajectory);
+  void GetOverheadTrajectory(const std::uint32_t matched_index,
+                             const std::uint32_t buffer_size,
+                             ADCTrajectory* trajectory_pb);
 
   apollo::common::util::Factory<PlanningConfig::PlannerType, Planner>
       planner_factory_;

modules/planning/planning_test.cc

@@ -29,7 +29,6 @@
 namespace apollo {
 namespace planning {
 
-using TrajectoryPb = ADCTrajectory;
 using apollo::common::TrajectoryPoint;
 
 class PlanningTest : public ::testing::Test {
@@ -50,36 +49,39 @@ TEST_F(PlanningTest, ComputeTrajectory) {
   common::VehicleState::instance()->set_linear_velocity(0.15);
   common::VehicleState::instance()->set_angular_velocity(0.0);
 
-  std::vector<TrajectoryPoint> trajectory1;
+  ADCTrajectory trajectory1;
   double time1 = 0.1;
   planning.Init();
   planning.Plan(false, time1, &trajectory1);
 
-  EXPECT_EQ(trajectory1.size(), (std::uint32_t)FLAGS_rtk_trajectory_forward);
-  const auto& p1_start = trajectory1.front();
-  const auto& p1_end = trajectory1.back();
+  EXPECT_EQ(trajectory1.trajectory_point_size(), FLAGS_rtk_trajectory_forward);
+  const auto& p1_start = trajectory1.trajectory_point().begin();
+  const auto& p1_end = trajectory1.trajectory_point().rbegin();
 
-  EXPECT_DOUBLE_EQ(p1_start.path_point().x(), 586385.782841);
-  EXPECT_DOUBLE_EQ(p1_end.path_point().x(), 586355.063786);
+  EXPECT_DOUBLE_EQ(p1_start->path_point().x(), 586385.782841);
+  EXPECT_DOUBLE_EQ(p1_end->path_point().x(), 586355.063786);
 
-  std::vector<TrajectoryPoint> trajectory2;
+  ADCTrajectory trajectory2;
   double time2 = 0.5;
   planning.Plan(true, time2, &trajectory2);
 
-  EXPECT_EQ(trajectory2.size(), (std::uint32_t)FLAGS_rtk_trajectory_forward +
-                                    (int)FLAGS_rtk_trajectory_backward);
+  EXPECT_EQ(trajectory2.trajectory_point_size(),
+            FLAGS_rtk_trajectory_forward + FLAGS_rtk_trajectory_backward);
 
-  const auto& p2_backward = trajectory2.front();
-  const auto& p2_start = trajectory2[FLAGS_rtk_trajectory_backward];
-  const auto& p2_end = trajectory2.back();
+  const auto& p2_backward = trajectory2.trajectory_point().begin();
+  const auto& p2_start =
+      trajectory2.trajectory_point(FLAGS_rtk_trajectory_backward);
+  const auto& p2_end = trajectory2.trajectory_point().rbegin();
 
-  EXPECT_DOUBLE_EQ(p2_backward.path_point().x(), 586385.577255);
+  EXPECT_DOUBLE_EQ(p2_backward->path_point().x(), 586385.577255);
   EXPECT_DOUBLE_EQ(p2_start.path_point().x(), 586385.486723);
-  EXPECT_DOUBLE_EQ(p2_end.path_point().x(), 586353.262913);
+  EXPECT_DOUBLE_EQ(p2_end->path_point().x(), 586353.262913);
 
-  double absolute_time1 = trajectory1[100].relative_time() + time1;
+  double absolute_time1 =
+      trajectory1.trajectory_point(100).relative_time() + time1;
   double absolute_time2 =
-      trajectory2[60 + FLAGS_rtk_trajectory_backward].relative_time() + time2;
+      trajectory2.trajectory_point(60 + FLAGS_rtk_trajectory_backward)
+          .relative_time() + time2;
 
   EXPECT_NEAR(absolute_time1, absolute_time2, 0.001);
 }
@@ -97,7 +99,7 @@ TEST_F(PlanningTest, ComputeTrajectoryNoRTKFile) {
   common::VehicleState::instance()->set_angular_velocity(0.0);
 
   double time = 0.1;
-  std::vector<TrajectoryPoint> trajectory;
+  ADCTrajectory trajectory;
   bool res_planning = planning.Plan(false, time, &trajectory);
   EXPECT_FALSE(res_planning);