Control: Add additional flags for test

modules/control/common/control_gflags.cc

@@ -49,3 +49,5 @@ DEFINE_bool(enable_gain_scheduler, false,
             "Enable gain scheduler for higher vechile speed");
 DEFINE_bool(set_steer_limit, false, "Set steer limit");
 DEFINE_bool(use_relative_position, false, "Use relative position");
+DEFINE_double(query_relative_time, 0.5,
+              "Temp flag to query target by relative time");

modules/control/common/control_gflags.h

@@ -46,5 +46,5 @@ DECLARE_double(steer_angle_rate);
 DECLARE_bool(enable_gain_scheduler);
 DECLARE_bool(set_steer_limit);
 DECLARE_bool(use_relative_position);
-
+DECLARE_double(query_relative_time);
 #endif  // MODULES_CONTROL_COMMON_CONTROL_GFLAGS_H_

modules/control/conf/control.conf

@@ -7,3 +7,5 @@
 --set_steer_limit=true
 --noenable_input_timestamp_check
 --use_relative_position=false
+--query_relative_time=0.5
+--use_ros_time=false

modules/control/controller/lat_controller.cc

@@ -543,37 +543,44 @@ void LatController::ComputeLateralErrors(
     const double x, const double y, const double theta, const double linear_v,
     const double angular_v, const TrajectoryAnalyzer &trajectory_analyzer,
     SimpleLateralDebug *debug) const {
-  auto matched_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);
+  // TODO: (QiL) change this to conf.
+  ::apollo::common::TrajectoryPoint target_point;
+  if (FLAGS_use_relative_position) {
+    target_point = trajectory_analyzer.QueryNearestPointByRelativeTime(
+        FLAGS_query_relative_time);
+  } else {
+    target_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);
+  }
 
-  double dx = x - matched_point.path_point().x();
-  double dy = y - matched_point.path_point().y();
+  double dx = x - target_point.path_point().x();
+  double dy = y - target_point.path_point().y();
 
   ADEBUG << "x point: " << x << " y point: " << y;
-  ADEBUG << "math point x: " << matched_point.path_point().x()
-         << " y point: " << matched_point.path_point().y();
+  ADEBUG << "math point x: " << target_point.path_point().x()
+         << " y point: " << target_point.path_point().y();
 
-  double cos_matched_theta = std::cos(matched_point.path_point().theta());
-  double sin_matched_theta = std::sin(matched_point.path_point().theta());
+  double cos_matched_theta = std::cos(target_point.path_point().theta());
+  double sin_matched_theta = std::sin(target_point.path_point().theta());
   // d_error = cos_matched_theta * dy - sin_matched_theta * dx;
   debug->set_lateral_error(cos_matched_theta * dy - sin_matched_theta * dx);
 
   double delta_theta =
-      common::math::NormalizeAngle(theta - matched_point.path_point().theta());
+      common::math::NormalizeAngle(theta - target_point.path_point().theta());
   double sin_delta_theta = std::sin(delta_theta);
   // d_error_dot = linear_v * sin_delta_theta;
   debug->set_lateral_error_rate(linear_v * sin_delta_theta);
 
   // theta_error = delta_theta;
   debug->set_heading_error(delta_theta);
-  // theta_error_dot = angular_v - matched_point.path_point().kappa() *
-  // matched_point.v();
-  debug->set_heading_error_rate(angular_v - matched_point.path_point().kappa() *
-                                                matched_point.v());
-
-  // matched_theta = matched_point.path_point().theta();
-  debug->set_ref_heading(matched_point.path_point().theta());
-  // matched_kappa = matched_point.path_point().kappa();
-  debug->set_curvature(matched_point.path_point().kappa());
+  // theta_error_dot = angular_v - target_point.path_point().kappa() *
+  // target_point.v();
+  debug->set_heading_error_rate(angular_v - target_point.path_point().kappa() *
+                                                target_point.v());
+
+  // matched_theta = target_point.path_point().theta();
+  debug->set_ref_heading(target_point.path_point().theta());
+  // matched_kappa = target_point.path_point().kappa();
+  debug->set_curvature(target_point.path_point().kappa());
 }
 
 }  // namespace control