added feed by file feature for VehicleState class

preparation for more unit tests

modules/common/vehicle_state/BUILD

@@ -14,6 +14,7 @@ cc_library(
         "//modules/canbus/proto:canbus_proto",
         "//modules/common:log",
         "//modules/common/math:quaternion",
+        "//modules/common/util",
         "//modules/localization/common:localization_common",
         "@eigen//:eigen",
     ],

modules/common/vehicle_state/vehicle_state.cc

@@ -20,6 +20,7 @@
 
 #include "modules/common/log.h"
 #include "modules/common/math/quaternion.h"
+#include "modules/common/util/file.h"
 #include "modules/common/vehicle_state/vehicle_state.h"
 #include "modules/localization/common/localization_gflags.h"
 
@@ -28,6 +29,23 @@ namespace common {
 
 VehicleState::VehicleState() {}
 
+bool VehicleState::Update(const std::string &localization_file,
+                          const std::string &chassis_file) {
+  localization::LocalizationEstimate localization;
+  if (!common::util::GetProtoFromFile(localization_file, &localization)) {
+    AERROR << "Failed to load file " << localization_file;
+    return false;
+  }
+
+  canbus::Chassis chassis;
+  if (!common::util::GetProtoFromFile(chassis_file, &chassis)) {
+    AERROR << "Failed to load file " << chassis_file;
+    return false;
+  }
+  Update(localization, chassis);
+  return true;
+}
+
 void VehicleState::Update(
     const localization::LocalizationEstimate &localization,
     const canbus::Chassis &chassis) {

modules/common/vehicle_state/vehicle_state.h

@@ -49,6 +49,15 @@ class VehicleState {
   void Update(const localization::LocalizationEstimate& localization,
               const canbus::Chassis& chassis);
 
+  /**
+   * @brief Update localization from protobuf files.
+   * @param localization The localization protobuf file
+   * @param chassis The chassis protobuf file
+   * @return true if update success.
+   */
+  bool Update(const std::string& localization_file,
+              const std::string& chassis_file);
+
   double timestamp() const { return timestamp_; };
 
   const apollo::localization::Pose& pose() const { return pose_; };

modules/planning/optimizer/BUILD

@@ -71,6 +71,7 @@ cc_library(
         "//modules/common:log",
         "//modules/common/adapters:adapter_manager",
         "//modules/common/util",
+        "//modules/common/vehicle_state",
         "//modules/planning/common:data_center",
         "//modules/planning/proto:dp_poly_path_config_proto",
         "@gtest//:gtest",

modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc

@@ -14,6 +14,8 @@
  * limitations under the License.
  *****************************************************************************/
 
+#include <algorithm>
+#include <cmath>
 #include <iostream>
 
 #include "gmock/gmock.h"
@@ -39,11 +41,13 @@ using common::adapter::AdapterManager;
 
 class DpRoadGraphTest : public OptimizerTestBase {
  public:
-  void SetInitPointWithVelocity(const double velocity) {
-    init_point_.mutable_path_point()->set_x(586392.84003);
-    init_point_.mutable_path_point()->set_y(4140673.01232);
-    init_point_.set_v(velocity);
-    init_point_.set_a(0.0);
+  void SetInitPoint() {
+    init_point_.mutable_path_point()->set_x(pose_.position().x());
+    init_point_.mutable_path_point()->set_y(pose_.position().y());
+    const auto& velocity = pose_.linear_velocity();
+    init_point_.set_v(std::hypot(velocity.x(), velocity.y()));
+    const auto& acc = pose_.linear_acceleration();
+    init_point_.set_a(std::hypot(acc.x(), acc.y()));
     init_point_.set_relative_time(0.0);
   }
 
@@ -66,6 +70,8 @@ class DpRoadGraphTest : public OptimizerTestBase {
   virtual void SetUp() {
     google::InitGoogleLogging("DpRoadGraphTest");
     OptimizerTestBase::SetUp();
+    SetInitPoint();
+    SetSpeedDataWithConstVeolocity(10.0);
     reference_line_ = &(frame_->planning_data().reference_line());
   }
 
@@ -78,17 +84,14 @@ class DpRoadGraphTest : public OptimizerTestBase {
 };
 
 TEST_F(DpRoadGraphTest, speed_road_graph) {
-  SetInitPointWithVelocity(10.0);
-  SetSpeedDataWithConstVeolocity(10.0);
-
   DpRoadGraph road_graph(dp_poly_path_config_, init_point_, speed_data_);
   ASSERT_TRUE(reference_line_ != nullptr);
   bool result =
       road_graph.find_tunnel(*reference_line_, &decision_data_, &path_data_);
   EXPECT_TRUE(result);
-  EXPECT_EQ(808, path_data_.discretized_path().num_of_points());
-  EXPECT_EQ(808, path_data_.frenet_frame_path().number_of_points());
-  EXPECT_FLOAT_EQ(80.0, path_data_.frenet_frame_path().points().back().s());
+  EXPECT_EQ(407, path_data_.discretized_path().num_of_points());
+  EXPECT_EQ(407, path_data_.frenet_frame_path().number_of_points());
+  EXPECT_FLOAT_EQ(40.0, path_data_.frenet_frame_path().points().back().s());
   EXPECT_FLOAT_EQ(0.0, path_data_.frenet_frame_path().points().back().l());
   // export_path_data(path_data_, "/tmp/path.txt");
 }

modules/planning/optimizer/optimizer_test_base.cc

@@ -17,6 +17,7 @@
 #include "modules/planning/optimizer/optimizer_test_base.h"
 
 #include "modules/common/log.h"
+#include "modules/common/vehicle_state/vehicle_state.h"
 
 namespace apollo {
 namespace planning {
@@ -26,6 +27,12 @@ using common::adapter::AdapterManager;
 DEFINE_string(test_routing_result_file,
               "modules/planning/testdata/garage_routing.pb.txt",
               "The routing file used in test");
+DEFINE_string(test_localization_file,
+              "modules/planning/testdata/garage_localization.pb.txt",
+              "The localization test file");
+DEFINE_string(test_chassis_file,
+              "modules/planning/testdata/garage_chassis.pb.txt",
+              "The chassis test file");
 
 void OptimizerTestBase::SetDataConfigs() {
   FLAGS_planning_config_file =
@@ -36,6 +43,9 @@ void OptimizerTestBase::SetDataConfigs() {
       "modules/planning/testdata/conf/reference_line_smoother_config.pb.txt";
   FLAGS_dp_poly_path_config_file =
       "modules/planning/testdata/conf/dp_poly_path_config.pb.txt";
+  FLAGS_test_localization_file =
+      "modules/planning/testdata/garage_localization.pb.txt";
+  FLAGS_test_chassis_file = "modules/planning/testdata/garage_chassis.pb.txt",
   FLAGS_v = 4;
   FLAGS_alsologtostderr = true;
 }
@@ -61,6 +71,16 @@ void OptimizerTestBase::SetUp() {
     AERROR << "Failed to load file " << FLAGS_dp_poly_path_config_file;
     return;
   }
+  if (AdapterManager::GetLocalization()->Empty() ||
+      AdapterManager::GetChassis()->Empty()) {
+    common::VehicleState::instance()->Update(FLAGS_test_localization_file,
+                                             FLAGS_test_chassis_file);
+  } else {
+    common::VehicleState::instance()->Update(
+        AdapterManager::GetLocalization()->GetLatestObserved(),
+        AdapterManager::GetChassis()->GetLatestObserved());
+  }
+  pose_ = common::VehicleState::instance()->pose();
   frame_ = data_center->current_frame();
   ASSERT_TRUE(frame_ != nullptr);
 }

modules/planning/optimizer/optimizer_test_base.h

@@ -35,6 +35,8 @@ namespace planning {
 using common::adapter::AdapterManager;
 
 DECLARE_string(test_routing_result_file);
+DECLARE_string(test_localization_file);
+DECLARE_string(test_chassis_file);
 
 class OptimizerTestBase : public ::testing::Test {
  public:
@@ -68,6 +70,7 @@ class OptimizerTestBase : public ::testing::Test {
 
  protected:
   DpPolyPathConfig dp_poly_path_config_;
+  localization::Pose pose_;
   Frame* frame_ = nullptr;
 };
 

modules/planning/testdata/garage_chassis.pb.txt

+engine_started: true
+engine_rpm: 0.0
+speed_mps: 0.0
+odometer_m: 0.0
+fuel_range_m: 0
+throttle_percentage: 15.2391853333
+brake_percentage: 13.6079959869
+steering_percentage: 5.65957450867
+steering_torque_nm: -0.3125
+parking_brake: false
+driving_mode: COMPLETE_MANUAL
+error_code: NO_ERROR
+gear_location: GEAR_NEUTRAL
+header {
+  timestamp_sec: 1498682801.33
+  module_name: "chassis"
+  sequence_num: 169202
+}
+signal {
+  turn_signal: TURN_NONE
+  horn: false
+}

modules/planning/testdata/garage_localization.pb.txt

+header {
+  timestamp_sec: 1498682801.33
+  module_name: "localization"
+  sequence_num: 168770
+}
+pose {
+  position {
+    x: 586387.203263
+    y: 4140676.21271
+    z: -29.1851183716
+  }
+  orientation {
+    qx: -0.00551346339344
+    qy: 0.0363995342758
+    qz: -0.620280830689
+    qw: -0.783515390216
+  }
+  linear_velocity {
+    x: 0.00149997781434
+    y: 0.000880812599767
+    z: -0.00122802127571
+  }
+  linear_acceleration {
+    x: -0.00582869976263
+    y: 0.0100514010432
+    z: 0.0235518746941
+  }
+  angular_velocity {
+    x: 0.000998771709994
+    y: 0.000154125122251
+    z: 1.03917169376e-05
+  }
+  heading: 2.90890211889
+  linear_acceleration_vrf {
+    x: 0.00994229889291
+    y: 0.00712407897059
+    z: 0.0232399095739
+  }
+  angular_velocity_vrf {
+    x: 0.000377985098552
+    y: -0.000936068612645
+    z: -4.80654498315e-05
+  }
+}