planning: added navigation mode test

modules/planning/common/frame.cc

@@ -77,6 +77,10 @@ const common::VehicleState &Frame::vehicle_state() const {
 }
 
 bool Frame::Rerouting() {
+  if (FLAGS_use_navigation_mode) {
+    AERROR << "Rerouting not supported in navigation mode";
+    return false;
+  }
   auto *adapter_manager = AdapterManager::instance();
   if (adapter_manager->GetRoutingResponse()->Empty()) {
     AERROR << "No previous routing available";
@@ -280,6 +284,9 @@ const Obstacle *Frame::CreateStaticVirtualObstacle(const std::string &id,
 }
 
 int Frame::CreateDestinationObstacle() {
+  if (FLAGS_use_navigation_mode) {
+    return 0;
+  }
   const auto &routing =
       AdapterManager::GetRoutingResponse()->GetLatestObserved();
   if (routing.routing_request().waypoint_size() < 2) {
@@ -410,9 +417,11 @@ void Frame::RecordInputDebug(planning_internal::Debug *debug) {
   auto debug_chassis = planning_data->mutable_chassis();
   debug_chassis->CopyFrom(chassis);
 
-  auto debug_routing = planning_data->mutable_routing();
-  debug_routing->CopyFrom(
-      AdapterManager::GetRoutingResponse()->GetLatestObserved());
+  if (!FLAGS_use_navigation_mode) {
+    auto debug_routing = planning_data->mutable_routing();
+    debug_routing->CopyFrom(
+        AdapterManager::GetRoutingResponse()->GetLatestObserved());
+  }
 
   planning_data->mutable_prediction_header()->CopyFrom(prediction_.header());
 }

modules/planning/integration_tests/BUILD

@@ -75,4 +75,19 @@ cc_test(
     ],
 )
 
+cc_test(
+    name = "navigation_mode_test",
+    size = "small",
+    srcs = [
+        "navigation_mode_test.cc",
+    ],
+    data = [
+        "//modules/common/configs:config_gflags",
+        "//modules/planning:planning_testdata",
+    ],
+    deps = [
+        ":planning_test_base",
+    ],
+)
+
 cpplint()

modules/planning/integration_tests/garage_test.cc

@@ -45,6 +45,7 @@ class GarageTest : public PlanningTestBase {
     FLAGS_base_map_filename = "base_map.txt";
     FLAGS_test_data_dir = "modules/planning/testdata/garage_test";
     FLAGS_planning_upper_speed_limit = 12.5;
+    FLAGS_test_routing_response_file = "garage_routing.pb.txt";
     FLAGS_enable_lag_prediction = false;
     FLAGS_enable_stop_sign = false;
   }

modules/planning/integration_tests/navigation_mode_test.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include <string>
+
+#include "gtest/gtest.h"
+
+#include "modules/common/configs/config_gflags.h"
+#include "modules/map/hdmap/hdmap_util.h"
+#include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/integration_tests/planning_test_base.h"
+#include "modules/planning/planning.h"
+
+namespace apollo {
+namespace planning {
+
+/**
+ * @class SunnyvaleLoopTest
+ * @brief This is an integration test that uses the sunnyvale_loop map.
+ */
+
+class NavigationModeTest : public PlanningTestBase {
+ public:
+  virtual void SetUp() {
+    FLAGS_use_navigation_mode = true;
+    FLAGS_test_data_dir = "modules/planning/testdata/navigation_mode_test";
+  }
+};
+
+/*
+ * test stop for not-nudgable obstacle
+ * A cruise test case
+ */
+TEST_F(NavigationModeTest, cruise) {
+  std::string seq_num = "1";
+  FLAGS_enable_prediction = false;
+  FLAGS_test_localization_file = seq_num + "_localization.pb.txt";
+  FLAGS_test_chassis_file = seq_num + "_chassis.pb.txt";
+  FLAGS_test_relative_map_file = seq_num + "_relative_map.pb.txt";
+  PlanningTestBase::SetUp();
+  RUN_GOLDEN_TEST(0);
+}
+}  // namespace planning
+}  // namespace apollo
+
+TMAIN;

modules/planning/integration_tests/planning_test_base.cc

@@ -31,16 +31,12 @@ using common::adapter::AdapterManager;
 DEFINE_string(test_data_dir, "", "the test data folder");
 DEFINE_bool(test_update_golden_log, false,
             "true to update decision golden log file.");
-DEFINE_string(test_routing_response_file, "garage_routing.pb.txt",
-              "The routing file used in test");
-DEFINE_string(test_localization_file, "garage_localization.pb.txt",
-              "The localization test file");
-DEFINE_string(test_chassis_file, "garage_chassis.pb.txt",
-              "The chassis test file");
+DEFINE_string(test_routing_response_file, "", "The routing file used in test");
+DEFINE_string(test_localization_file, "", "The localization test file");
+DEFINE_string(test_chassis_file, "", "The chassis test file");
 DEFINE_string(test_prediction_file, "", "The prediction module test file");
 DEFINE_string(test_traffic_light_file, "", "The traffic light test file");
 DEFINE_string(test_relative_map_file, "", "The relative map test file");
-
 DEFINE_string(test_previous_planning_file, "",
               "The previous planning test file");
 
@@ -51,9 +47,10 @@ void PlanningTestBase::SetUpTestCase() {
   FLAGS_smoother_config_filename =
       "modules/planning/conf/smoother_config.pb.txt";
   FLAGS_map_dir = "modules/planning/testdata";
-  FLAGS_test_localization_file = "garage_localization.pb.txt";
-  FLAGS_test_chassis_file = "garage_chassis.pb.txt";
-  FLAGS_test_prediction_file = "garage_prediction.pb.txt";
+  FLAGS_test_localization_file = "";
+  FLAGS_test_chassis_file = "";
+  FLAGS_test_routing_response_file = "";
+  FLAGS_test_prediction_file = "";
   FLAGS_align_prediction_time = false;
   FLAGS_estimate_current_vehicle_state = false;
   FLAGS_enable_reference_line_provider_thread = false;
@@ -62,20 +59,23 @@ void PlanningTestBase::SetUpTestCase() {
   FLAGS_enable_lag_prediction = false;
 }
 
-#define FEED_ADAPTER(TYPE, FILENAME)                                         \
-  if (!AdapterManager::Get##TYPE()) {                                        \
-    AERROR << #TYPE                                                          \
-        " is not registered in adapter manager, check adapter file "         \
-           << FLAGS_planning_adapter_config_filename;                        \
-    return false;                                                            \
-  }                                                                          \
-  if (!FILENAME.empty() && !AdapterManager::Feed##TYPE##File(                \
-                               FLAGS_test_data_dir + "/" + FILENAME)) {      \
-    AERROR << "Failed to feed " #TYPE " file " << FLAGS_test_data_dir << "/" \
-           << FILENAME;                                                      \
-    return false;                                                            \
-  }                                                                          \
-  AINFO << "Using " #TYPE << " provided by " << FILENAME;
+#define FEED_ADAPTER(TYPE, FILENAME)                                           \
+  if (!AdapterManager::Get##TYPE()) {                                          \
+    AERROR << #TYPE                                                            \
+        " is not registered in adapter manager, check adapter file "           \
+           << FLAGS_planning_adapter_config_filename;                          \
+    return false;                                                              \
+  }                                                                            \
+  if (!FILENAME.empty()) {                                                     \
+    if (!AdapterManager::Feed##TYPE##File(FLAGS_test_data_dir + "/" +          \
+                                          FILENAME)) {                         \
+      AERROR << "Failed to feed " #TYPE " file " << FLAGS_test_data_dir << "/" \
+             << FILENAME;                                                      \
+      return false;                                                            \
+    }                                                                          \
+    AINFO << "Using " #TYPE << " provided by " << FLAGS_test_data_dir << "/"   \
+          << FILENAME;                                                         \
+  }
 
 bool PlanningTestBase::SetUpAdapters() {
   if (!AdapterManager::Initialized()) {

modules/planning/integration_tests/planning_test_base.h

@@ -55,6 +55,7 @@ DECLARE_string(test_chassis_file);
 DECLARE_string(test_data_dir);
 DECLARE_string(test_prediction_file);
 DECLARE_string(test_traffic_light_file);
+DECLARE_string(test_relative_map_file);
 DECLARE_string(test_previous_planning_file);
 
 class PlanningTestBase : public ::testing::Test {

modules/planning/planning.cc

@@ -177,8 +177,13 @@ void Planning::RunOnce() {
 
   const double start_timestamp = Clock::NowInSeconds();
 
-  hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
-  CHECK(hdmap_) << "Failed to load map";
+  if (FLAGS_use_navigation_mode) {
+    // hdmap is created on the fly with relative map
+    hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
+    CHECK(hdmap_) << "Failed to load map";
+    reference_line_provider_ = std::unique_ptr<ReferenceLineProvider>(
+        new ReferenceLineProvider(hdmap_, config_.smoother_type()));
+  }
 
   ADCTrajectory not_ready_pb;
   auto* not_ready = not_ready_pb.mutable_decision()
@@ -188,7 +193,8 @@ void Planning::RunOnce() {
     not_ready->set_reason("localization not ready");
   } else if (AdapterManager::GetChassis()->Empty()) {
     not_ready->set_reason("chassis not ready");
-  } else if (AdapterManager::GetRoutingResponse()->Empty()) {
+  } else if (!FLAGS_use_navigation_mode &&
+             AdapterManager::GetRoutingResponse()->Empty()) {
     not_ready->set_reason("routing not ready");
   }
   if (not_ready->has_reason()) {
@@ -234,7 +240,8 @@ void Planning::RunOnce() {
     return;
   }
 
-  if (!reference_line_provider_->UpdateRoutingResponse(
+  if (!FLAGS_use_navigation_mode &&
+      !reference_line_provider_->UpdateRoutingResponse(
           AdapterManager::GetRoutingResponse()->GetLatestObserved())) {
     std::string msg("Failed to update routing in reference line provider");
     AERROR << msg;
@@ -249,7 +256,9 @@ void Planning::RunOnce() {
   }
 
   // Update reference line provider
-  reference_line_provider_->UpdateVehicleState(vehicle_state);
+  if (!FLAGS_use_navigation_mode) {
+    reference_line_provider_->UpdateVehicleState(vehicle_state);
+  }
 
   const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
   bool is_replan = false;

modules/planning/reference_line/reference_line_provider.cc

@@ -199,8 +199,13 @@ void ReferenceLineProvider::GenerateThread() {
 }
 
 double ReferenceLineProvider::LastTimeDelay() {
-  std::lock_guard<std::mutex> lock(reference_lines_mutex_);
-  return last_calculation_time_;
+  if (FLAGS_enable_reference_line_provider_thread &&
+      !FLAGS_use_navigation_mode) {
+    std::lock_guard<std::mutex> lock(reference_lines_mutex_);
+    return last_calculation_time_;
+  } else {
+    return last_calculation_time_;
+  }
 }
 
 bool ReferenceLineProvider::GetReferenceLines(
@@ -210,12 +215,15 @@ bool ReferenceLineProvider::GetReferenceLines(
   CHECK_NOTNULL(segments);
 
   if (FLAGS_use_navigation_mode) {
+    double start_time = Clock::NowInSeconds();
     bool result = GetReferenceLinesFromRelativeMap(
         AdapterManager::GetRelativeMap()->GetLatestObserved(), reference_lines,
         segments);
     if (!result) {
       AERROR << "Failed to get reference line from relative map";
     }
+    double end_time = Clock::NowInSeconds();
+    last_calculation_time_ = end_time - start_time;
     return result;
   }
 
@@ -260,7 +268,7 @@ bool ReferenceLineProvider::GetReferenceLinesFromRelativeMap(
     const relative_map::MapMsg &relative_map,
     std::list<ReferenceLine> *reference_line,
     std::list<hdmap::RouteSegments> *segments) {
-  if (!relative_map.navigation_path_size() <= 0) {
+  if (relative_map.navigation_path_size() <= 0) {
     return false;
   }
   auto *hdmap = HDMapUtil::BaseMapPtr();

modules/planning/testdata/navigation_mode_test/1_chassis.pb.txt

+engine_started: true
+engine_rpm: 1550.25
+speed_mps: 9.86944484711
+odometer_m: 0.0
+fuel_range_m: 0
+throttle_percentage: 22.0416564941
+brake_percentage: 14.0566110611
+steering_percentage: -0.0425531901419
+steering_torque_nm: 0.375
+parking_brake: false
+driving_mode: EMERGENCY_MODE
+error_code: NO_ERROR
+gear_location: GEAR_DRIVE
+header {
+  timestamp_sec: 1518218588.45
+  module_name: "chassis"
+  sequence_num: 238704
+}
+signal {
+  turn_signal: TURN_NONE
+  horn: false
+}
+chassis_gps {
+  latitude: 37.4124346667
+  longitude: -122.009781333
+  gps_valid: true
+  year: 18
+  month: 2
+  day: 9
+  hours: 23
+  minutes: 23
+  seconds: 7
+  compass_direction: 180.0
+  pdop: 0.6
+  is_gps_fault: false
+  is_inferred: false
+  altitude: -42.5
+  heading: 195.1
+  hdop: 1.2
+  vdop: 1.4
+  quality: FIX_3D
+  num_satellites: 10
+  gps_speed: 9.83488
+}
+engage_advice {
+  advice: READY_TO_ENGAGE
+}

modules/planning/testdata/navigation_mode_test/1_localization.pb.txt

+header {
+  timestamp_sec: 1518218588.47
+  module_name: "localization"
+  sequence_num: 238280
+}
+pose {
+  position {
+    x: 587624.606385
+    y: 4141076.38626
+    z: -31.3435181957
+  }
+  orientation {
+    qx: 0.0218437023901
+    qy: -0.00311970993868
+    qz: -0.99206419174
+    qw: -0.123781095254
+  }
+  linear_velocity {
+    x: -2.52061123257
+    y: -9.58588342104
+    z: 0.023528923181
+  }
+  linear_acceleration {
+    x: 0.0188544060246
+    y: -0.0652903096148
+    z: 0.355984944507
+  }
+  angular_velocity {
+    x: -0.00506093940716
+    y: 0.0136247181945
+    z: -0.00498992822065
+  }
+  heading: -1.81907311521
+  linear_acceleration_vrf {
+    x: -0.0499884670918
+    y: 0.0589336097809
+    z: 0.354078489417
+  }
+  angular_velocity_vrf {
+    x: 0.00846548658694
+    y: -0.0119672026885
+    z: -0.00461161944097
+  }
+  euler_angles {
+    x: 0.0441273652206
+    y: -0.000782230304849
+    z: -1.81907311521
+  }
+}
+measurement_time: 1518218588.46