common: skip localization pose update in case of use_navigation_mode.

modules/common/vehicle_state/BUILD

@@ -13,6 +13,7 @@ cc_library(
     deps = [
         "//modules/common:log",
         "//modules/common:macro",
+        "//modules/common/configs:config_gflags",
         "//modules/common/math:box2d",
         "//modules/common/math:quaternion",
         "//modules/common/proto:vehicle_state_proto",

modules/common/vehicle_state/vehicle_state_provider.cc

@@ -20,6 +20,7 @@
 
 #include "Eigen/Core"
 
+#include "modules/common/configs/config_gflags.h"
 #include "modules/common/log.h"
 #include "modules/common/math/euler_angles_zxy.h"
 #include "modules/common/math/quaternion.h"
@@ -34,6 +35,7 @@ VehicleStateProvider::VehicleStateProvider() {}
 Status VehicleStateProvider::Update(
     const localization::LocalizationEstimate &localization,
     const canbus::Chassis &chassis) {
+  original_localization_ = localization;
   if (!ConstructExceptLinearVelocity(localization)) {
     std::string msg = util::StrCat(
         "Fail to update because ConstructExceptLinearVelocity error.",
@@ -68,6 +70,12 @@ bool VehicleStateProvider::ConstructExceptLinearVelocity(
     AERROR << "Invalid localization input.";
     return false;
   }
+  // skip localization update when it is in use_navigation_mode.
+  if (FLAGS_use_navigation_mode) {
+    ADEBUG << "Skip localization update when it is in use_navigation_mode.";
+    return true;
+  }
+
   vehicle_state_.mutable_pose()->CopyFrom(localization.pose());
   if (localization.pose().has_position()) {
     vehicle_state_.set_x(localization.pose().position().x());
@@ -172,6 +180,10 @@ const localization::Pose &VehicleStateProvider::pose() const {
   return vehicle_state_.pose();
 }
 
+const localization::Pose &VehicleStateProvider::original_pose() const {
+  return original_localization_.pose();
+}
+
 void VehicleStateProvider::set_linear_velocity(const double linear_velocity) {
   vehicle_state_.set_linear_velocity(linear_velocity);
 }

modules/common/vehicle_state/vehicle_state_provider.h

@@ -67,6 +67,7 @@ class VehicleStateProvider {
   double timestamp() const;
 
   const localization::Pose& pose() const;
+  const localization::Pose& original_pose() const;
 
   /**
    * @brief Default destructor.
@@ -176,6 +177,7 @@ class VehicleStateProvider {
       const localization::LocalizationEstimate& localization);
 
   common::VehicleState vehicle_state_;
+  localization::LocalizationEstimate original_localization_;
 
   DECLARE_SINGLETON(VehicleStateProvider);
 };

modules/localization/proto/localization.proto

@@ -51,7 +51,7 @@ message LocalizationEstimate {
 
   // The time of pose measurement, seconds since the GPS epoch (Jan 6, 1980).
   optional double measurement_time = 4;  // In seconds.
-  
+
   // Future trajectory acturally driven by the drivers
   repeated apollo.common.TrajectoryPoint trajectory_point = 5;
 }