diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index d7e33c97a458f04c28f071326d6e329aeb41e85e..e9bebc5484284e23267a2829f97b7c9a599f1dab 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/modules/prediction/common/prediction_gflags.cc @@ -147,5 +147,5 @@ DEFINE_double(sample_time_gap, 0.2, "Gap of time to sample time to get to the lane center"); DEFINE_double(cost_alpha, 100.0, "The coefficient of lateral acceleration in cost function"); -DEFINE_double(default_time_to_end_state, 5.0, +DEFINE_double(default_time_to_lat_end_state, 5.0, "The default time to lane center"); diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 9bad6afff3333fad52f7e31a935a85bacfb2f1df..33370f587212592024456227ddeeefcc335f33a7 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/modules/prediction/common/prediction_gflags.h @@ -102,6 +102,6 @@ DECLARE_double(time_upper_bound_to_lane_center); DECLARE_double(time_lower_bound_to_lane_center); DECLARE_double(sample_time_gap); DECLARE_double(cost_alpha); -DECLARE_double(default_time_to_end_state); +DECLARE_double(default_time_to_lat_end_state); #endif // MODULES_PREDICTION_COMMON_PREDICTION_GFLAGS_H_ diff --git a/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc b/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc index d773e02197f82eb7fc984c9b2277ceb5a25c90cc..43ed15774703c0ababed41a0c5e0de20d9b46fa2 100644 --- a/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc +++ b/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc @@ -115,16 +115,16 @@ void MoveSequencePredictor::DrawMoveSequenceTrajectoryPoints( } Eigen::Vector2d position(feature.position().x(), feature.position().y()); - double time_to_end_state = - std::max(FLAGS_default_time_to_end_state, - ComputeTimeToLaneCenterByVelocity(obstacle, lane_sequence)); + double time_to_lat_end_state = std::max(FLAGS_default_time_to_lat_end_state, + ComputeTimeToLatEndConditionByVelocity(obstacle, lane_sequence)); + // double time_to_lon_end_state = + // ComputeTimeToLonEndCondition(obstacle, lane_sequence); std::array lateral_coeffs; std::array longitudinal_coeffs; - GetLateralPolynomial(obstacle, lane_sequence, time_to_end_state, + GetLateralPolynomial(obstacle, lane_sequence, time_to_lat_end_state, &lateral_coeffs); - GetLongitudinalPolynomial(obstacle, lane_sequence, time_to_end_state, - &longitudinal_coeffs); + GetLongitudinalPolynomial(obstacle, lane_sequence, &longitudinal_coeffs); int lane_segment_index = 0; std::string lane_id = @@ -140,12 +140,12 @@ void MoveSequencePredictor::DrawMoveSequenceTrajectoryPoints( double prev_lane_l = lane_l; size_t total_num = static_cast(total_time / period); - size_t num_to_center = static_cast(time_to_end_state / period); + size_t num_to_lat_end = static_cast(time_to_lat_end_state / period); for (size_t i = 0; i < total_num; ++i) { double relative_time = static_cast(i) * period; Eigen::Vector2d point; double theta = M_PI; - if (i < num_to_center) { + if (i < num_to_lat_end) { lane_l = EvaluateQuinticPolynomial(lateral_coeffs, relative_time, 0); } else { lane_l = 0.0; @@ -197,10 +197,13 @@ void MoveSequencePredictor::DrawMoveSequenceTrajectoryPoints( void MoveSequencePredictor::GetLongitudinalPolynomial( const Obstacle& obstacle, const LaneSequence& lane_sequence, - const double time_to_end_state, std::array* coefficients) { + std::array* coefficients) { CHECK_GT(obstacle.history_size(), 0); CHECK_GT(lane_sequence.lane_segment_size(), 0); CHECK_GT(lane_sequence.lane_segment(0).lane_point_size(), 0); + std::pair lon_end_state = + ComputeLonEndState(obstacle, lane_sequence); + double time_to_end_state = lon_end_state.second; const Feature& feature = obstacle.latest_feature(); double theta = feature.velocity_heading(); double v = feature.speed(); @@ -215,7 +218,8 @@ void MoveSequencePredictor::GetLongitudinalPolynomial( double ds0 = v * std::cos(theta - lane_heading); double dds0 = a * std::cos(theta - lane_heading); double min_end_speed = std::min(FLAGS_still_obstacle_speed_threshold, ds0); - double ds1 = std::max(min_end_speed, ds0 + dds0 * time_to_end_state); + // double ds1 = std::max(min_end_speed, ds0 + dds0 * time_to_end_state); + double ds1 = lon_end_state.first; double dds1 = 0.0; double p = time_to_end_state; @@ -274,13 +278,13 @@ void MoveSequencePredictor::GetLateralPolynomial( coefficients->operator[](5) = (6.0 * c0 - 3.0 * c1 + 0.5 * c2) / p2; } -double MoveSequencePredictor::ComputeTimeToLaneCenterBySampling( +double MoveSequencePredictor::ComputeTimeToLatEndConditionBySampling( const Obstacle& obstacle, const LaneSequence& lane_sequence) { std::vector candidate_times; GenerateCandidateTimes(&candidate_times); if (candidate_times.empty()) { AWARN << "No candidate times found, use default value."; - return FLAGS_default_time_to_end_state; + return FLAGS_default_time_to_lat_end_state; } double t_best = candidate_times[0]; double cost_min = std::numeric_limits::max(); @@ -288,7 +292,7 @@ double MoveSequencePredictor::ComputeTimeToLaneCenterBySampling( std::array lateral_coeffs; std::array longitudinal_coeffs; GetLateralPolynomial(obstacle, lane_sequence, t, &lateral_coeffs); - GetLongitudinalPolynomial(obstacle, lane_sequence, t, &longitudinal_coeffs); + GetLongitudinalPolynomial(obstacle, lane_sequence, &longitudinal_coeffs); double cost = Cost(t, lateral_coeffs, longitudinal_coeffs); if (cost < cost_min) { t_best = t; @@ -298,7 +302,7 @@ double MoveSequencePredictor::ComputeTimeToLaneCenterBySampling( return t_best; } -double MoveSequencePredictor::ComputeTimeToLaneCenterByVelocity( +double MoveSequencePredictor::ComputeTimeToLatEndConditionByVelocity( const Obstacle& obstacle, const LaneSequence& lane_sequence) { CHECK_GT(obstacle.history_size(), 0); CHECK_GT(lane_sequence.lane_segment_size(), 0); @@ -319,6 +323,13 @@ double MoveSequencePredictor::ComputeTimeToLaneCenterByVelocity( return std::fabs(lane_l / v_l); } +std::pair MoveSequencePredictor::ComputeLonEndState( + const Obstacle& obstacle, const LaneSequence& lane_sequence) { + // TODO(kechxu) implement + double t = 5.0; + return {0.0, t}; +} + double MoveSequencePredictor::Cost( const double t, const std::array& lateral_coeffs, const std::array& longitudinal_coeffs) { diff --git a/modules/prediction/predictor/move_sequence/move_sequence_predictor.h b/modules/prediction/predictor/move_sequence/move_sequence_predictor.h index 6ccbb76889bd55d36d742db01e94fd8cb3b448e5..0074b7418f8520929b8f29d6a028cd9e24ce9890 100644 --- a/modules/prediction/predictor/move_sequence/move_sequence_predictor.h +++ b/modules/prediction/predictor/move_sequence/move_sequence_predictor.h @@ -25,6 +25,7 @@ #include #include #include +#include #include "Eigen/Dense" #include "modules/common/math/kalman_filter.h" @@ -61,7 +62,6 @@ class MoveSequencePredictor : public SequencePredictor { void GetLongitudinalPolynomial(const Obstacle& obstacle, const LaneSequence& lane_sequence, - const double time_to_end_state, std::array* coefficients); void GetLateralPolynomial(const Obstacle& obstacle, @@ -69,11 +69,14 @@ class MoveSequencePredictor : public SequencePredictor { const double time_to_end_state, std::array* coefficients); - double ComputeTimeToLaneCenterBySampling(const Obstacle& obstacle, - const LaneSequence& lane_sequence); + double ComputeTimeToLatEndConditionBySampling( + const Obstacle& obstacle, const LaneSequence& lane_sequence); - double ComputeTimeToLaneCenterByVelocity(const Obstacle& obstacle, - const LaneSequence& lane_sequence); + double ComputeTimeToLatEndConditionByVelocity( + const Obstacle& obstacle, const LaneSequence& lane_sequence); + + std::pair ComputeLonEndState( + const Obstacle& obstacle, const LaneSequence& lane_sequence); double Cost(const double t, const std::array& lateral_coeffs, const std::array& longitudinal_coeffs);