From f22761bf1cf165c466ed0f6c2809879480c89dc3 Mon Sep 17 00:00:00 2001
From: Yajia Zhang <zhangyajia@baidu.com>
Date: Wed, 2 Aug 2017 11:18:00 -0700
Subject: [PATCH] Fixed the trajecotry publish timestamp

---
 modules/planning/planning.cc                  | 21 ++++++++++++-------
 modules/planning/planning.h                   |  3 ++-
 .../trajectory_stitcher.cc                    |  7 ++++---
 .../trajectory_stitcher/trajectory_stitcher.h |  3 ++-
 4 files changed, 22 insertions(+), 12 deletions(-)

diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc
index 5c1117890d..22df0974e3 100644
--- a/modules/planning/planning.cc
+++ b/modules/planning/planning.cc
@@ -201,12 +201,15 @@ void Planning::RunOnce() {
 
   common::VehicleState::instance()->Update(localization, chassis);
 
-  double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
+  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
+
+  /**
   // the execution_start_time is the estimated time when the planned trajectory
   // will be executed by the controller.
   double execution_start_time =
       apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) +
       planning_cycle_time;
+  **/
 
   ADCTrajectory trajectory_pb;
   RecordInput(&trajectory_pb);
@@ -218,7 +221,8 @@ void Planning::RunOnce() {
   }
 
   bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
-  bool res_planning = Plan(is_auto_mode, execution_start_time, &trajectory_pb);
+  bool res_planning = Plan(is_auto_mode, start_timestamp, planning_cycle_time,
+      &trajectory_pb);
 
   const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
   const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
@@ -227,7 +231,7 @@ void Planning::RunOnce() {
 
   if (res_planning) {
     AdapterManager::FillPlanningHeader("planning", &trajectory_pb);
-    trajectory_pb.mutable_header()->set_timestamp_sec(execution_start_time);
+    trajectory_pb.mutable_header()->set_timestamp_sec(start_timestamp);
     AdapterManager::PublishPlanning(trajectory_pb);
     ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString();
   } else {
@@ -237,12 +241,15 @@ void Planning::RunOnce() {
 
 void Planning::Stop() {}
 
-bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
+bool Planning::Plan(const bool is_on_auto_mode,
+                    const double current_time_stamp,
+                    const double planning_cycle_time,
                     ADCTrajectory* trajectory_pb) {
-  double vehicle_state_time = VehicleState::instance()->timestamp();
+
   const auto& stitching_trajectory =
       TrajectoryStitcher::compute_stitching_trajectory(
-          is_on_auto_mode, vehicle_state_time, last_publishable_trajectory_);
+          is_on_auto_mode, current_time_stamp, planning_cycle_time,
+          last_publishable_trajectory_);
 
   frame_->SetPlanningStartPoint(stitching_trajectory.back());
 
@@ -309,7 +316,7 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
   publishable_trajectory.prepend_trajectory_points(
       stitching_trajectory.begin(), stitching_trajectory.end() - 1);
 
-  publishable_trajectory.set_header_time(vehicle_state_time);
+  publishable_trajectory.set_header_time(current_time_stamp);
 
   publishable_trajectory.populate_trajectory_protobuf(trajectory_pb);
 
diff --git a/modules/planning/planning.h b/modules/planning/planning.h
index 457b72ff7e..98b9753f58 100644
--- a/modules/planning/planning.h
+++ b/modules/planning/planning.h
@@ -77,7 +77,8 @@ class Planning : public apollo::common::ApolloApp {
    * @param trajectory_pb the computed planning trajectory
    */
   bool Plan(const bool is_on_auto_mode,
-            const double publish_time,
+            const double current_time_stamp,
+            const double planning_cycle_time,
             ADCTrajectory* trajectory_pb);
 
   const Frame* GetFrame() const;
diff --git a/modules/planning/trajectory_stitcher/trajectory_stitcher.cc b/modules/planning/trajectory_stitcher/trajectory_stitcher.cc
index a126835290..d1c7451357 100644
--- a/modules/planning/trajectory_stitcher/trajectory_stitcher.cc
+++ b/modules/planning/trajectory_stitcher/trajectory_stitcher.cc
@@ -51,7 +51,8 @@ std::vector<TrajectoryPoint> compute_reinit_stitching_trajectory() {
 //    3. the position deviation from actual and target is too high
 std::vector<TrajectoryPoint> TrajectoryStitcher::compute_stitching_trajectory(
     const bool is_on_auto_mode,
-    const double vehicle_state_time,
+    const double current_timestamp,
+    const double planning_cycle_time,
     const PublishableTrajectory& prev_trajectory) {
 
   if (!is_on_auto_mode) {
@@ -68,7 +69,7 @@ std::vector<TrajectoryPoint> TrajectoryStitcher::compute_stitching_trajectory(
   }
 
   const double veh_rel_time =
-      vehicle_state_time - prev_trajectory.header_time();
+      current_timestamp - prev_trajectory.header_time();
 
   std::size_t matched_index = prev_trajectory.query_nearest_point(veh_rel_time);
 
@@ -96,7 +97,7 @@ std::vector<TrajectoryPoint> TrajectoryStitcher::compute_stitching_trajectory(
     return compute_reinit_stitching_trajectory();
   }
 
-  double forward_rel_time = veh_rel_time + FLAGS_forward_predict_time;
+  double forward_rel_time = veh_rel_time + planning_cycle_time;
   std::size_t forward_index =
       prev_trajectory.query_nearest_point(forward_rel_time);
 
diff --git a/modules/planning/trajectory_stitcher/trajectory_stitcher.h b/modules/planning/trajectory_stitcher/trajectory_stitcher.h
index eda6fc2cee..12cc41949f 100644
--- a/modules/planning/trajectory_stitcher/trajectory_stitcher.h
+++ b/modules/planning/trajectory_stitcher/trajectory_stitcher.h
@@ -38,7 +38,8 @@ class TrajectoryStitcher {
 
   static std::vector<TrajectoryPoint> compute_stitching_trajectory(
       const bool is_on_auto_mode,
-      const double vehicle_state_time,
+      const double current_timestamp,
+      const double planning_cycle_time,
       const PublishableTrajectory& prev_trajectory);
 };
 
-- 
GitLab