changed vehicle_state to singleton.

modules/common/vehicle_state/vehicle_state.cc

@@ -24,19 +24,9 @@ namespace apollo {
 namespace common {
 namespace vehicle_state {
 
-VehicleState::VehicleState(
-    const localization::LocalizationEstimate &localization) {
-  ConstructExceptLinearVelocity(&localization);
-  if (localization.has_pose() && localization.pose().has_linear_velocity()) {
-    double linear_v_x = localization.pose().linear_velocity().x();
-    double linear_v_y = localization.pose().linear_velocity().y();
-    double linear_v_z = localization.pose().linear_velocity().z();
-    linear_v_ = std::hypot(std::hypot(linear_v_x, linear_v_y), linear_v_z);
-  }
-  gear_ = ::apollo::canbus::Chassis::GEAR_NONE;
-}
+VehicleState::VehicleState() {}
 
-VehicleState::VehicleState(
+void VehicleState::Update(
     const localization::LocalizationEstimate *localization,
     const canbus::Chassis *chassis) {
   ConstructExceptLinearVelocity(localization);

modules/common/vehicle_state/vehicle_state.h

@@ -21,10 +21,12 @@
 #ifndef MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_H_
 #define MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_H_
 
+#include "Eigen/Core"
+
 #include "modules/canbus/proto/chassis.pb.h"
 #include "modules/localization/proto/localization.pb.h"
 
-#include "Eigen/Core"
+#include "modules/common/macro.h"
 
 /**
  * @namespace apollo::common::vehicle_state
@@ -42,24 +44,13 @@ namespace vehicle_state {
  */
 class VehicleState {
  public:
-  /**
-   * @brief Empty constructor.
-   */
-  VehicleState() = default;
-
-  /**
-   * @brief Constructor only by information of localization.
-   * @param localization Localization information of the vehicle.
-   */
-  explicit VehicleState(const localization::LocalizationEstimate &localization);
-
   /**
    * @brief Constructor by information of localization and chassis.
    * @param localization Localization information of the vehicle.
    * @param chassis Chassis information of the vehicle.
    */
-  VehicleState(const localization::LocalizationEstimate *localization,
-               const canbus::Chassis *chassis);
+  void Update(const localization::LocalizationEstimate* localization,
+              const canbus::Chassis* chassis);
 
   double timestamp() const { return timestamp_; };
 
@@ -181,26 +172,21 @@ class VehicleState {
   Eigen::Vector2d ComputeCOMPosition(const double rear_to_com_distance) const;
 
  private:
+  DECLARE_SINGLETON(VehicleState);
+
   void ConstructExceptLinearVelocity(
-      const localization::LocalizationEstimate *localization);
+      const localization::LocalizationEstimate* localization);
 
   double x_ = 0.0;
-
   double y_ = 0.0;
-
   double z_ = 0.0;
-
   double heading_ = 0.0;
-
   double linear_v_ = 0.0;
-
   double angular_v_ = 0.0;
-
   double linear_a_ = 0.0;
 
   ::apollo::canbus::Chassis::GearPosition gear_;
-
-  const localization::LocalizationEstimate *localization_ptr_ = nullptr;
+  const localization::LocalizationEstimate* localization_ptr_ = nullptr;
 
   double timestamp_ = 0.0;
 };

modules/common/vehicle_state/vehicle_state_test.cc

@@ -50,21 +50,23 @@ class VehicleStateTest : public ::testing::Test {
 };
 
 TEST_F(VehicleStateTest, Accessors) {
-  VehicleState vehicle_state(&localization_, &chassis_);
-  EXPECT_DOUBLE_EQ(vehicle_state.x(), 357.51331791372041);
-  EXPECT_DOUBLE_EQ(vehicle_state.y(), 96.165912376788725);
-  EXPECT_DOUBLE_EQ(vehicle_state.heading(), -1.8388082455104939);
-  EXPECT_DOUBLE_EQ(vehicle_state.linear_velocity(), 3.0);
-  EXPECT_DOUBLE_EQ(vehicle_state.angular_velocity(), -0.0079623083093763921);
-  EXPECT_DOUBLE_EQ(vehicle_state.linear_acceleration(), -0.079383290718229638);
+  auto* vehicle_state = VehicleState::instance();
+  vehicle_state->Update(&localization_, &chassis_);
+  EXPECT_DOUBLE_EQ(vehicle_state->x(), 357.51331791372041);
+  EXPECT_DOUBLE_EQ(vehicle_state->y(), 96.165912376788725);
+  EXPECT_DOUBLE_EQ(vehicle_state->heading(), -1.8388082455104939);
+  EXPECT_DOUBLE_EQ(vehicle_state->linear_velocity(), 3.0);
+  EXPECT_DOUBLE_EQ(vehicle_state->angular_velocity(), -0.0079623083093763921);
+  EXPECT_DOUBLE_EQ(vehicle_state->linear_acceleration(), -0.079383290718229638);
 }
 
 TEST_F(VehicleStateTest, EstimateFuturePosition) {
-  VehicleState vehicle_state(&localization_, &chassis_);
-  Eigen::Vector2d future_position = vehicle_state.EstimateFuturePosition(1.0);
+  auto* vehicle_state = VehicleState::instance();
+  vehicle_state->Update(&localization_, &chassis_);
+  Eigen::Vector2d future_position = vehicle_state->EstimateFuturePosition(1.0);
   EXPECT_NEAR(future_position[0], 356.707, 1e-3);
   EXPECT_NEAR(future_position[1], 93.276, 1e-3);
-  future_position = vehicle_state.EstimateFuturePosition(2.0);
+  future_position = vehicle_state->EstimateFuturePosition(2.0);
   EXPECT_NEAR(future_position[0], 355.879, 1e-3);
   EXPECT_NEAR(future_position[1], 90.393, 1e-3);
 }

modules/control/controller/lat_controller.cc

@@ -121,17 +121,18 @@ void LatController::ProcessLogs(const SimpleLateralDebug *debug,
                                 const canbus::Chassis *chassis) {
   std::stringstream log_stream;
   log_stream << debug->lateral_error() << "," << debug->ref_heading() << ","
-             << vehicle_state_.heading() << "," << debug->heading_error() << ","
-             << debug->heading_error_rate() << ","
-             << debug->lateral_error_rate() << "," << debug->curvature() << ","
-             << debug->steer_angle() << "," << debug->steer_angle_feedforward()
-             << "," << debug->steer_angle_lateral_contribution() << ","
+             << VehicleState::instance()->heading() << ","
+             << debug->heading_error() << "," << debug->heading_error_rate()
+             << "," << debug->lateral_error_rate() << "," << debug->curvature()
+             << "," << debug->steer_angle() << ","
+             << debug->steer_angle_feedforward() << ","
+             << debug->steer_angle_lateral_contribution() << ","
              << debug->steer_angle_lateral_rate_contribution() << ","
              << debug->steer_angle_heading_contribution() << ","
              << debug->steer_angle_heading_rate_contribution() << ","
              << debug->steer_angle_feedback() << ","
              << chassis->steering_percentage() << ","
-             << vehicle_state_.linear_velocity();
+             << VehicleState::instance()->linear_velocity();
   if (FLAGS_enable_csv_debug) {
     steer_log_file_ << log_stream.str() << std::endl;
   }
@@ -232,9 +233,9 @@ Status LatController::ComputeControlCommand(
     const canbus::Chassis *chassis,
     const planning::ADCTrajectory *planning_published_trajectory,
     ControlCommand *cmd) {
-  vehicle_state_ = std::move(VehicleState(localization, chassis));
-  vehicle_state_.set_linear_velocity(
-      std::max(vehicle_state_.linear_velocity(), 1.0));
+  VehicleState::instance()->Update(localization, chassis);
+  VehicleState::instance()->set_linear_velocity(
+      std::max(VehicleState::instance()->linear_velocity(), 1.0));
 
   trajectory_analyzer_ =
       std::move(TrajectoryAnalyzer(planning_published_trajectory));
@@ -271,11 +272,12 @@ Status LatController::ComputeControlCommand(
   // Clamp the steer angle to -100.0 to 100.0
   steer_angle = apollo::common::math::Clamp(steer_angle, -100.0, 100.0);
 
-  double steer_limit = std::atan(max_lat_acc_ * wheelbase_ /
-                                 (vehicle_state_.linear_velocity() *
-                                  vehicle_state_.linear_velocity())) *
-                       steer_transmission_ratio_ * 180 / M_PI /
-                       steer_single_direction_max_degree_;
+  double steer_limit =
+      std::atan(max_lat_acc_ * wheelbase_ /
+                (VehicleState::instance()->linear_velocity() *
+                 VehicleState::instance()->linear_velocity())) *
+      steer_transmission_ratio_ * 180 / M_PI /
+      steer_single_direction_max_degree_;
 
   // Clamp the steer angle
   double steer_angle_limited =
@@ -304,7 +306,7 @@ Status LatController::ComputeControlCommand(
 
   // TODO(yifei): move up temporary values to use debug fields.
 
-  debug->set_heading(vehicle_state_.heading());
+  debug->set_heading(VehicleState::instance()->heading());
   debug->set_steer_angle(steer_angle);
   debug->set_steer_angle_feedforward(steer_angle_feedforward);
   debug->set_steer_angle_lateral_contribution(steer_angle_lateral_contribution);
@@ -315,7 +317,7 @@ Status LatController::ComputeControlCommand(
       steer_angle_heading_rate_contribution);
   debug->set_steer_angle_feedback(steer_angle_feedback);
   debug->set_steering_position(chassis->steering_percentage());
-  debug->set_ref_speed(vehicle_state_.linear_velocity());
+  debug->set_ref_speed(VehicleState::instance()->linear_velocity());
   debug->set_steer_angle_limited(steer_angle_limited);
   ProcessLogs(debug, chassis);
   return Status::OK();
@@ -331,7 +333,7 @@ Status LatController::Reset() {
 // Rate, Preview Lateral1, Preview Lateral2, ...]
 void LatController::UpdateState(SimpleLateralDebug *debug) {
   TrajectoryPoint traj_point;
-  Eigen::Vector2d com = vehicle_state_.ComputeCOMPosition(lr_);
+  Eigen::Vector2d com = VehicleState::instance()->ComputeCOMPosition(lr_);
   double raw_lateral_error = GetLateralError(com, &traj_point);
 
   // lateral_error_ = lateral_rate_filter_.Filter(raw_lateral_error);
@@ -344,12 +346,14 @@ void LatController::UpdateState(SimpleLateralDebug *debug) {
   debug->set_ref_heading(traj_point.path_point().theta());
 
   // heading_error_ =
-  //    common::math::NormalizeAngle(vehicle_state_.heading() - ref_heading_);
+  //    common::math::NormalizeAngle(VehicleState::instance()->heading() -
+  //    ref_heading_);
   debug->set_heading_error(common::math::NormalizeAngle(
-      vehicle_state_.heading() - traj_point.path_point().theta()));
+      VehicleState::instance()->heading() - traj_point.path_point().theta()));
 
   // Reverse heading error if vehicle is going in reverse
-  if (vehicle_state_.gear() == ::apollo::canbus::Chassis::GEAR_REVERSE) {
+  if (VehicleState::instance()->gear() ==
+      ::apollo::canbus::Chassis::GEAR_REVERSE) {
     debug->set_heading_error(-debug->heading_error());
   }
 
@@ -375,7 +379,7 @@ void LatController::UpdateState(SimpleLateralDebug *debug) {
   for (int i = 0; i < preview_window_; ++i) {
     double preview_time = ts_ * (i + 1);
     Eigen::Vector2d future_position_estimate =
-        vehicle_state_.EstimateFuturePosition(preview_time);
+        VehicleState::instance()->EstimateFuturePosition(preview_time);
     double preview_lateral = GetLateralError(future_position_estimate, nullptr);
     matrix_state_(basic_state_size_ + i, 0) = preview_lateral;
   }
@@ -383,11 +387,11 @@ void LatController::UpdateState(SimpleLateralDebug *debug) {
 }
 
 void LatController::UpdateStateAnalyticalMatching(SimpleLateralDebug *debug) {
-  Eigen::Vector2d com = vehicle_state_.ComputeCOMPosition(lr_);
-  ComputeLateralErrors(com.x(), com.y(), vehicle_state_.heading(),
-                       vehicle_state_.linear_velocity(),
-                       vehicle_state_.angular_velocity(), trajectory_analyzer_,
-                       debug);
+  Eigen::Vector2d com = VehicleState::instance()->ComputeCOMPosition(lr_);
+  ComputeLateralErrors(com.x(), com.y(), VehicleState::instance()->heading(),
+                       VehicleState::instance()->linear_velocity(),
+                       VehicleState::instance()->angular_velocity(),
+                       trajectory_analyzer_, debug);
 
   // State matrix update;
   // First four elements are fixed;
@@ -417,7 +421,7 @@ void LatController::UpdateStateAnalyticalMatching(SimpleLateralDebug *debug) {
 }
 
 void LatController::UpdateMatrix() {
-  double v = vehicle_state_.linear_velocity();
+  double v = VehicleState::instance()->linear_velocity();
   matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
   matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
   matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;
@@ -445,7 +449,7 @@ double LatController::ComputeFeedForward(double ref_curvature) const {
       lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;
 
   // then change it from rad to %
-  double v = vehicle_state_.linear_velocity();
+  double v = VehicleState::instance()->linear_velocity();
   double steer_angle_feedforwardterm =
       (wheelbase_ * ref_curvature + kv * v * v * ref_curvature -
        matrix_k_(0, 2) *

modules/control/controller/lat_controller.h

@@ -122,9 +122,6 @@ class LatController : public Controller {
 
   void CloseLogFile();
 
-  // a proxy to access vehicle movement state
-  ::apollo::common::vehicle_state::VehicleState vehicle_state_;
-
   // a proxy to analyze the planning trajectory
   TrajectoryAnalyzer trajectory_analyzer_;
 

modules/control/controller/lat_controller_test.cc

@@ -90,8 +90,6 @@ class LatControllerTest : public ::testing::Test, LatController {
 
   LatControllerConf lateral_conf_;
 
-  std::unique_ptr<VehicleState> vehicle_state_;
-
   double timestamp_ = 0.0;
 };
 
@@ -102,7 +100,8 @@ TEST_F(LatControllerTest, ComputeLateralErrors) {
   auto chassis_pb = LoadChassisPb(
       "modules/control/testdata/longitudinal_controller_test/1_chassis.pb.txt");
   FLAGS_enable_map_reference_unify = false;
-  VehicleState vehicle_state(&localization_pb, &chassis_pb);
+  auto *vehicle_state = VehicleState::instance();
+  vehicle_state->Update(&localization_pb, &chassis_pb);
 
   auto planning_trajectory_pb = LoadPlanningTrajectoryPb(
       "modules/control/testdata/longitudinal_controller_test/"
@@ -112,10 +111,10 @@ TEST_F(LatControllerTest, ComputeLateralErrors) {
   ControlCommand cmd;
   SimpleLateralDebug *debug = cmd.mutable_debug()->mutable_simple_lat_debug();
 
-  ComputeLateralErrors(vehicle_state.x(), vehicle_state.y(),
-                       vehicle_state.heading(), vehicle_state.linear_velocity(),
-                       vehicle_state.angular_velocity(), trajectory_analyzer,
-                       debug);
+  ComputeLateralErrors(
+      vehicle_state->x(), vehicle_state->y(), vehicle_state->heading(),
+      vehicle_state->linear_velocity(), vehicle_state->angular_velocity(),
+      trajectory_analyzer, debug);
 
   double theta_error_expected = -0.03549;
   double theta_error_dot_expected = 0.0044552856731;

modules/control/controller/lon_controller.cc

@@ -154,7 +154,7 @@ Status LonController::ComputeControlCommand(
     ::apollo::control::ControlCommand *cmd) {
   localization_ = localization;
   chassis_ = chassis;
-  vehicle_state_ = std::move(VehicleState(localization, chassis));
+  VehicleState::instance()->Update(localization, chassis);
 
   trajectory_message_ = planning_published_trajectory;
   if (!control_interpolation_) {
@@ -184,8 +184,7 @@ Status LonController::ComputeControlCommand(
     return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
                   "Invalid preview time:" + std::to_string(preview_time));
   }
-  ComputeLongitudinalErrors(vehicle_state_, trajectory_analyzer_.get(),
-                            preview_time, debug);
+  ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, debug);
 
   double station_error_limit = lon_controller_conf.station_error_limit();
   double station_error_limited = 0.0;
@@ -214,7 +213,8 @@ Status LonController::ComputeControlCommand(
       speed_controller_input_limit);
 
   double acceleration_cmd_closeloop = 0.0;
-  if (vehicle_state_.linear_velocity() <= lon_controller_conf.switch_speed()) {
+  if (VehicleState::instance()->linear_velocity() <=
+      lon_controller_conf.switch_speed()) {
     speed_pid_controller_.SetPID(lon_controller_conf.low_speed_pid_conf());
     acceleration_cmd_closeloop =
         speed_pid_controller_.Control(speed_controller_input_limited, ts);
@@ -285,7 +285,7 @@ Status LonController::ComputeControlCommand(
   cmd->set_throttle(throttle_cmd);
   cmd->set_brake(brake_cmd);
 
-  if (std::abs(vehicle_state_.linear_velocity()) <=
+  if (std::abs(VehicleState::instance()->linear_velocity()) <=
           FLAGS_max_abs_speed_when_stopped ||
       chassis->gear_location() == trajectory_message_->gear() ||
       chassis->gear_location() == ::apollo::canbus::Chassis::GEAR_NEUTRAL) {
@@ -306,7 +306,6 @@ Status LonController::Reset() {
 std::string LonController::Name() const { return name_; }
 
 void LonController::ComputeLongitudinalErrors(
-    const VehicleState &vehicle_state,
     const TrajectoryAnalyzer *trajectory_analyzer, const double preview_time,
     SimpleLongitudinalDebug *debug) {
   // the decomposed vehicle motion onto Frenet frame
@@ -320,11 +319,12 @@ void LonController::ComputeLongitudinalErrors(
   double d_dot_matched = 0.0;
 
   auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(
-      vehicle_state.x(), vehicle_state.y());
+      VehicleState::instance()->x(), VehicleState::instance()->y());
 
   trajectory_analyzer->ToTrajectoryFrame(
-      vehicle_state.x(), vehicle_state.y(), vehicle_state.heading(),
-      vehicle_state.linear_velocity(), matched_point, &s_matched,
+      VehicleState::instance()->x(), VehicleState::instance()->y(),
+      VehicleState::instance()->heading(),
+      VehicleState::instance()->linear_velocity(), matched_point, &s_matched,
       &s_dot_matched, &d_matched, &d_dot_matched);
 
   double current_control_time = apollo::common::time::ToSecond(Clock::Now());

modules/control/controller/lon_controller.h

@@ -99,10 +99,9 @@ class LonController : public Controller {
   std::string Name() const override;
 
  protected:
-  void ComputeLongitudinalErrors(
-      const ::apollo::common::vehicle_state::VehicleState &vehicle_state,
-      const TrajectoryAnalyzer *trajectory, const double preview_time,
-      SimpleLongitudinalDebug *debug);
+  void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory,
+                                 const double preview_time,
+                                 SimpleLongitudinalDebug *debug);
 
  private:
   void SetDigitalFilterAcceleration(
@@ -122,7 +121,6 @@ class LonController : public Controller {
 
   const ::apollo::localization::LocalizationEstimate *localization_ = nullptr;
   const ::apollo::canbus::Chassis *chassis_ = nullptr;
-  ::apollo::common::vehicle_state::VehicleState vehicle_state_;
 
   std::unique_ptr<Interpolation2D> control_interpolation_;
   const ::apollo::planning::ADCTrajectory *trajectory_message_ = nullptr;

modules/control/controller/lon_controller_test.cc

@@ -64,12 +64,10 @@ class LonControllerTest : public ::testing::Test, LonController {
     controller_.reset(new LonController());
   }
 
-  void ComputeLongitudinalErrors(
-      const ::apollo::common::vehicle_state::VehicleState &vehicle_state,
-      const TrajectoryAnalyzer *trajectory, const double preview_time,
-      SimpleLongitudinalDebug *debug) {
-    LonController::ComputeLongitudinalErrors(vehicle_state, trajectory,
-                                             preview_time, debug);
+  void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory,
+                                 const double preview_time,
+                                 SimpleLongitudinalDebug *debug) {
+    LonController::ComputeLongitudinalErrors(trajectory, preview_time, debug);
   }
 
   Status Init(const ControlConf *control_conf) {
@@ -119,15 +117,15 @@ TEST_F(LonControllerTest, ComputeLongitudinalErrors) {
   double time_now = apollo::common::time::ToSecond(Clock::Now());
   trajectory_pb.mutable_header()->set_timestamp_sec(time_now);
 
-  VehicleState vehicle_state(&localization_pb, &chassis_pb);
+  auto *vehicle_state = VehicleState::instance();
+  vehicle_state->Update(&localization_pb, &chassis_pb);
   TrajectoryAnalyzer trajectory_analyzer(&trajectory_pb);
 
   double ts = longitudinal_conf_.ts();
   double preview_time = longitudinal_conf_.preview_window() * ts;
 
   SimpleLongitudinalDebug debug;
-  ComputeLongitudinalErrors(vehicle_state, &trajectory_analyzer, preview_time,
-                            &debug);
+  ComputeLongitudinalErrors(&trajectory_analyzer, preview_time, &debug);
 
   double station_reference_expected = 0.16716666937000002;
   double speed_reference_expected = 1.70833337307;

modules/planning/common/planning_data.h

@@ -63,6 +63,8 @@ class PlanningData {
   PathData* mutable_path_data();
   SpeedData* mutable_speed_data();
 
+  double timestamp() const;
+
  protected:
   std::unique_ptr<ReferenceLine> _reference_line;
   std::unique_ptr<DecisionData> _decision_data;
@@ -70,6 +72,7 @@ class PlanningData {
   TrajectoryPoint _init_planning_point;
   PathData _path_data;
   SpeedData _speed_data;
+  double timestamp_ = 0.0;
 };
 
 }  // namespace planning

modules/planning/planning.cc

@@ -18,6 +18,7 @@
 
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/time/time.h"
+#include "modules/common/vehicle_state/vehicle_state.h"
 #include "modules/planning/common/data_center.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/planner/em/em_planner.h"
@@ -27,7 +28,6 @@ namespace apollo {
 namespace planning {
 
 using apollo::common::TrajectoryPoint;
-using apollo::common::vehicle_state::VehicleState;
 using apollo::common::adapter::AdapterManager;
 using apollo::common::time::Clock;
 using apollo::common::Status;
@@ -104,9 +104,10 @@ void Planning::RunOnce() {
 
   const auto& localization =
       AdapterManager::GetLocalization()->GetLatestObserved();
-  VehicleState vehicle_state(localization);
-
   const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
+
+  common::vehicle_state::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;
@@ -120,8 +121,8 @@ void Planning::RunOnce() {
       AdapterManager::GetPlanning()->GetSeqNum() + 1);
 
   std::vector<TrajectoryPoint> planning_trajectory;
-  bool res_planning = Plan(vehicle_state, is_on_auto_mode, execution_start_time,
-                           &planning_trajectory);
+  bool res_planning =
+      Plan(is_on_auto_mode, execution_start_time, &planning_trajectory);
   if (res_planning) {
     ADCTrajectory trajectory_pb =
         ToADCTrajectory(execution_start_time, planning_trajectory);
@@ -134,9 +135,8 @@ void Planning::RunOnce() {
 
 void Planning::Stop() {}
 
-bool Planning::Plan(const common::vehicle_state::VehicleState& vehicle_state,
-                    const bool is_on_auto_mode, const double publish_time,
-                    std::vector<TrajectoryPoint> *planning_trajectory) {
+bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
+                    std::vector<TrajectoryPoint>* planning_trajectory) {
   double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
   double execution_start_time = publish_time;
 
@@ -156,8 +156,10 @@ bool Planning::Plan(const common::vehicle_state::VehicleState& vehicle_state,
     // position and target vehicle position.
     // If the deviation exceeds a specific threshold,
     // it will be unsafe to planning from the matched point.
-    double dx = matched_point.path_point().x() - vehicle_state.x();
-    double dy = matched_point.path_point().y() - vehicle_state.y();
+    double dx = matched_point.path_point().x() -
+                common::vehicle_state::VehicleState::instance()->x();
+    double dy = matched_point.path_point().y() -
+                common::vehicle_state::VehicleState::instance()->y();
     double position_deviation = std::sqrt(dx * dx + dy * dy);
 
     if (position_deviation < FLAGS_replanning_threshold) {
@@ -191,7 +193,7 @@ bool Planning::Plan(const common::vehicle_state::VehicleState& vehicle_state,
   //    3. the position deviation from actual and target is too high
   // then planning from current vehicle state.
   TrajectoryPoint vehicle_state_point =
-      ComputeStartingPointFromVehicleState(vehicle_state, planning_cycle_time);
+      ComputeStartingPointFromVehicleState(planning_cycle_time);
 
   auto status =
       planner_->MakePlan(vehicle_state_point, planning_trajectory);
@@ -208,7 +210,7 @@ bool Planning::Plan(const common::vehicle_state::VehicleState& vehicle_state,
 std::pair<TrajectoryPoint, std::uint32_t>
 Planning::ComputeStartingPointFromLastTrajectory(
     const double start_time) const {
-  auto comp = [](const TrajectoryPoint &p, const double t) {
+  auto comp = [](const TrajectoryPoint& p, const double t) {
     return p.relative_time() < t;
   };
 
@@ -223,23 +225,28 @@ Planning::ComputeStartingPointFromLastTrajectory(
 }
 
 TrajectoryPoint Planning::ComputeStartingPointFromVehicleState(
-    const common::vehicle_state::VehicleState &vehicle_state,
     const double forward_time) const {
   // Eigen::Vector2d estimated_position =
-  // vehicle_state.EstimateFuturePosition(forward_time);
+  // VehicleState::instance()->EstimateFuturePosition(forward_time);
   TrajectoryPoint point;
   // point.set_x(estimated_position.x());
   // point.set_y(estimated_position.y());
-  point.mutable_path_point()->set_x(vehicle_state.x());
-  point.mutable_path_point()->set_y(vehicle_state.y());
-  point.mutable_path_point()->set_z(vehicle_state.z());
-  point.set_v(vehicle_state.linear_velocity());
-  point.set_a(vehicle_state.linear_acceleration());
+  point.mutable_path_point()->set_x(
+      common::vehicle_state::VehicleState::instance()->x());
+  point.mutable_path_point()->set_y(
+      common::vehicle_state::VehicleState::instance()->y());
+  point.mutable_path_point()->set_z(
+      common::vehicle_state::VehicleState::instance()->z());
+  point.set_v(
+      common::vehicle_state::VehicleState::instance()->linear_velocity());
+  point.set_a(
+      common::vehicle_state::VehicleState::instance()->linear_acceleration());
   point.mutable_path_point()->set_kappa(0.0);
   const double speed_threshold = 0.1;
   if (point.v() > speed_threshold) {
-    point.mutable_path_point()->set_kappa(vehicle_state.angular_velocity() /
-                                          vehicle_state.linear_velocity());
+    point.mutable_path_point()->set_kappa(
+        common::vehicle_state::VehicleState::instance()->angular_velocity() /
+        common::vehicle_state::VehicleState::instance()->linear_velocity());
   }
   point.mutable_path_point()->set_dkappa(0.0);
   point.mutable_path_point()->set_s(0.0);
@@ -263,7 +270,7 @@ std::vector<TrajectoryPoint> Planning::GetOverheadTrajectory(
 
   double zero_relative_time = last_trajectory_[matched_index].relative_time();
   // reset relative time
-  for (auto &p : overhead_trajectory) {
+  for (auto& p : overhead_trajectory) {
     p.set_relative_time(p.relative_time() - zero_relative_time);
   }
   return overhead_trajectory;

modules/planning/planning.h

@@ -18,8 +18,8 @@
 #define MODULES_PLANNING_PLANNING_H_
 
 #include <memory>
-#include <utility>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "modules/common/apollo_app.h"
@@ -72,14 +72,10 @@ class Planning : public apollo::common::ApolloApp {
 
   /**
    * @brief Plan the trajectory given current vehicle state
-   * @param vehicle_state variable describes the vehicle state, including
-   * position,
-   *        velocity, acceleration, heading, etc
    * @param is_on_auto_mode whether the current system is on auto-driving mode
    * @param publishable_trajectory the computed planning trajectory
    */
-  bool Plan(const common::vehicle_state::VehicleState &vehicle_state,
-            const bool is_on_auto_mode, const double publish_time,
+  bool Plan(const bool is_on_auto_mode, const double publish_time,
             std::vector<common::TrajectoryPoint> *discretized_trajectory);
 
   /**
@@ -95,7 +91,6 @@ class Planning : public apollo::common::ApolloApp {
   ComputeStartingPointFromLastTrajectory(const double curr_time) const;
 
   common::TrajectoryPoint ComputeStartingPointFromVehicleState(
-      const common::vehicle_state::VehicleState &vehicle_state,
       const double forward_time) const;
 
   std::vector<common::TrajectoryPoint> GetOverheadTrajectory(
@@ -103,7 +98,7 @@ class Planning : public apollo::common::ApolloApp {
 
   ADCTrajectory ToADCTrajectory(
       const double header_time,
-      const std::vector<common::TrajectoryPoint>& discretized_trajectory);
+      const std::vector<common::TrajectoryPoint> &discretized_trajectory);
 
   apollo::common::util::Factory<PlanningConfig::PlannerType, Planner>
       planner_factory_;

modules/planning/planning_test.cc

@@ -15,13 +15,16 @@
  *****************************************************************************/
 
 #include "modules/planning/planning.h"
-#include "modules/common/adapters/adapter_gflags.h"
-#include "modules/planning/common/planning_gflags.h"
-#include "modules/planning/proto/planning.pb.h"
 
 #include "gmock/gmock.h"
 #include "gtest/gtest.h"
 
+#include "modules/planning/proto/planning.pb.h"
+
+#include "modules/common/adapters/adapter_gflags.h"
+#include "modules/common/vehicle_state/vehicle_state.h"
+#include "modules/planning/common/planning_gflags.h"
+
 namespace apollo {
 namespace planning {
 
@@ -39,18 +42,17 @@ class PlanningTest : public ::testing::Test {
 TEST_F(PlanningTest, ComputeTrajectory) {
   FLAGS_rtk_trajectory_filename = "modules/planning/testdata/garage.csv";
   Planning planning;
-  common::vehicle_state::VehicleState vehicle_state;
-  vehicle_state.set_x(586385.782841);
-  vehicle_state.set_y(4140674.76065);
+  common::vehicle_state::VehicleState::instance()->set_x(586385.782841);
+  common::vehicle_state::VehicleState::instance()->set_y(4140674.76065);
 
-  vehicle_state.set_heading(2.836888814);
-  vehicle_state.set_linear_velocity(0.15);
-  vehicle_state.set_angular_velocity(0.0);
+  common::vehicle_state::VehicleState::instance()->set_heading(2.836888814);
+  common::vehicle_state::VehicleState::instance()->set_linear_velocity(0.15);
+  common::vehicle_state::VehicleState::instance()->set_angular_velocity(0.0);
 
   std::vector<TrajectoryPoint> trajectory1;
   double time1 = 0.1;
   planning.Init();
-  planning.Plan(vehicle_state, false, time1, &trajectory1);
+  planning.Plan(false, time1, &trajectory1);
 
   EXPECT_EQ(trajectory1.size(), (std::uint32_t)FLAGS_rtk_trajectory_forward);
   const auto& p1_start = trajectory1.front();
@@ -61,14 +63,14 @@ TEST_F(PlanningTest, ComputeTrajectory) {
 
   std::vector<TrajectoryPoint> trajectory2;
   double time2 = 0.5;
-  planning.Plan(vehicle_state, true, time2, &trajectory2);
+  planning.Plan(true, time2, &trajectory2);
 
   EXPECT_EQ(trajectory2.size(), (std::uint32_t)FLAGS_rtk_trajectory_forward +
                                     (int)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.front();
+  const auto& p2_start = trajectory2[FLAGS_rtk_trajectory_backward];
+  const auto& p2_end = trajectory2.back();
 
   EXPECT_DOUBLE_EQ(p2_backward.path_point().x(), 586385.577255);
   EXPECT_DOUBLE_EQ(p2_start.path_point().x(), 586385.486723);
@@ -86,17 +88,16 @@ TEST_F(PlanningTest, ComputeTrajectoryNoRTKFile) {
   Planning planning;
   planning.Init();
 
-  common::vehicle_state::VehicleState vehicle_state;
-  vehicle_state.set_x(586385.782841);
-  vehicle_state.set_y(4140674.76065);
+  common::vehicle_state::VehicleState::instance()->set_x(586385.782841);
+  common::vehicle_state::VehicleState::instance()->set_y(4140674.76065);
 
-  vehicle_state.set_heading(2.836888814);
-  vehicle_state.set_linear_velocity(0.0);
-  vehicle_state.set_angular_velocity(0.0);
+  common::vehicle_state::VehicleState::instance()->set_heading(2.836888814);
+  common::vehicle_state::VehicleState::instance()->set_linear_velocity(0.0);
+  common::vehicle_state::VehicleState::instance()->set_angular_velocity(0.0);
 
   double time = 0.1;
   std::vector<TrajectoryPoint> trajectory;
-  bool res_planning = planning.Plan(vehicle_state, false, time, &trajectory);
+  bool res_planning = planning.Plan(false, time, &trajectory);
   EXPECT_FALSE(res_planning);
 
   // check Reset runs gracefully.