diff --git a/modules/control/common/control_gflags.cc b/modules/control/common/control_gflags.cc index e35342ce7eed931b7d9d0e5770e07c4fedc3a3ef..46dfa15e4f04f0d6a1ca82e423d7c3e587bd9c25 100644 --- a/modules/control/common/control_gflags.cc +++ b/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"); diff --git a/modules/control/common/control_gflags.h b/modules/control/common/control_gflags.h index 332d990e012c7792d5b2336c393dc997388f6de6..04239d4f50a3391069c02ac51bc71e1019b7d3ed 100644 --- a/modules/control/common/control_gflags.h +++ b/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_ diff --git a/modules/control/conf/control.conf b/modules/control/conf/control.conf index 584f56b17e60543a47b4239e8b16a68cb09d0326..34642911ad6e11e53e5b512a171bd945901c9372 100644 --- a/modules/control/conf/control.conf +++ b/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 diff --git a/modules/control/controller/lat_controller.cc b/modules/control/controller/lat_controller.cc index 4928e587a4f3b21db7d5d3cd9ecf99119d8515bd..e99207afe4239219ada94bd65612d0de99f73a7b 100644 --- a/modules/control/controller/lat_controller.cc +++ b/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