From 5fb3ca0908af976410dbac0df4d89425b059f5f8 Mon Sep 17 00:00:00 2001 From: kechxu Date: Thu, 13 Dec 2018 20:42:50 -0800 Subject: [PATCH] Prediction: implement const acc trajectory for stop sign --- .../prediction/common/prediction_gflags.cc | 4 ++ modules/prediction/common/prediction_gflags.h | 2 + .../lane_sequence/lane_sequence_predictor.cc | 26 +++++-- .../move_sequence/move_sequence_predictor.cc | 23 +++++-- modules/prediction/predictor/predictor.cc | 28 ++++---- modules/prediction/predictor/predictor.h | 22 ++---- .../predictor/sequence/sequence_predictor.cc | 68 +++++++++++++++++++ .../predictor/sequence/sequence_predictor.h | 15 ++++ 8 files changed, 149 insertions(+), 39 deletions(-) diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index 9567e4d9e6..97b58e3bc0 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/modules/prediction/common/prediction_gflags.cc @@ -73,6 +73,8 @@ DEFINE_double(r_var, 0.25, "Measurement noise covariance"); DEFINE_double(p_var, 0.1, "Error covariance"); DEFINE_double(go_approach_rate, 0.995, "The rate to approach to the reference line of going straight"); +DEFINE_double(cutin_approach_rate, 0.95, + "The rate to approach to the reference line of cutin"); DEFINE_int32(min_still_obstacle_history_length, 4, "Min # historical frames for still obstacles"); @@ -155,6 +157,8 @@ DEFINE_double(distance_beyond_junction, 0.5, "consider it in junction."); DEFINE_double(defualt_junction_range, 10.0, "Default value for the range of a junction."); +DEFINE_double(distance_to_slow_down_at_stop_sign, 40.0, + "The distance to slow down at stop sign"); // Evaluator DEFINE_double(time_to_center_if_not_reach, 10.0, diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 32b12585fe..faa7f0c4a9 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/modules/prediction/common/prediction_gflags.h @@ -53,6 +53,7 @@ DECLARE_double(q_var); DECLARE_double(r_var); DECLARE_double(p_var); DECLARE_double(go_approach_rate); +DECLARE_double(cutin_approach_rate); DECLARE_int32(min_still_obstacle_history_length); DECLARE_int32(max_still_obstacle_history_length); @@ -99,6 +100,7 @@ DECLARE_double(centripetal_acc_coeff); DECLARE_double(junction_exit_lane_threshold); DECLARE_double(distance_beyond_junction); DECLARE_double(defualt_junction_range); +DECLARE_double(distance_to_slow_down_at_stop_sign); // Evaluator DECLARE_double(time_to_center_if_not_reach); diff --git a/modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc b/modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc index 0981baf11e..e5884a6160 100644 --- a/modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc +++ b/modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc @@ -74,9 +74,23 @@ void LaneSequencePredictor::Predict(Obstacle* obstacle) { << "] with probability [" << sequence.probability() << "]."; std::vector points; - DrawLaneSequenceTrajectoryPoints(*obstacle, sequence, - FLAGS_prediction_trajectory_time_length, - FLAGS_prediction_trajectory_time_resolution, &points); + bool is_about_to_stop = false; + double acceleration = 0.0; + if (sequence.has_stop_sign()) { + double stop_distance = sequence.stop_sign().lane_sequence_s() - + sequence.lane_s(); + is_about_to_stop = SupposedToStop(feature, stop_distance, &acceleration); + } + + if (is_about_to_stop) { + DrawConstantAccelerationTrajectory( + *obstacle, sequence, FLAGS_prediction_trajectory_time_length, + FLAGS_prediction_trajectory_time_resolution, acceleration, &points); + } else { + DrawLaneSequenceTrajectoryPoints(*obstacle, sequence, + FLAGS_prediction_trajectory_time_length, + FLAGS_prediction_trajectory_time_resolution, &points); + } if (points.empty()) { continue; @@ -121,6 +135,10 @@ void LaneSequencePredictor::DrawLaneSequenceTrajectoryPoints( AERROR << "Failed in getting lane s and lane l"; return; } + double approach_rate = FLAGS_go_approach_rate; + if (!lane_sequence.vehicle_on_lane()) { + approach_rate = FLAGS_cutin_approach_rate; + } size_t total_num = static_cast(total_time / period); for (size_t i = 0; i < total_num; ++i) { double relative_time = static_cast(i) * period; @@ -154,7 +172,7 @@ void LaneSequencePredictor::DrawLaneSequenceTrajectoryPoints( lane_id = lane_sequence.lane_segment(lane_segment_index).lane_id(); } - lane_l *= FLAGS_go_approach_rate; + lane_l *= approach_rate; } } diff --git a/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc b/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc index a50c4ba1e3..bcbea8d4ad 100644 --- a/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc +++ b/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc @@ -90,17 +90,30 @@ void MoveSequencePredictor::Predict(Obstacle* obstacle) { << "] with probability [" << sequence.probability() << "]."; std::vector points; - auto end_time1 = std::chrono::system_clock::now(); - DrawMoveSequenceTrajectoryPointsUsingBestTrajectorySelection( - *obstacle, sequence, FLAGS_prediction_trajectory_time_length, - FLAGS_prediction_trajectory_time_resolution, &points); + + bool is_about_to_stop = false; + double acceleration = 0.0; + if (sequence.has_stop_sign()) { + double stop_distance = sequence.stop_sign().lane_sequence_s() - + sequence.lane_s(); + is_about_to_stop = SupposedToStop(feature, stop_distance, &acceleration); + } + + if (is_about_to_stop) { + DrawConstantAccelerationTrajectory( + *obstacle, sequence, FLAGS_prediction_trajectory_time_length, + FLAGS_prediction_trajectory_time_resolution, acceleration, &points); + } else { + DrawMoveSequenceTrajectoryPointsUsingBestTrajectorySelection( + *obstacle, sequence, FLAGS_prediction_trajectory_time_length, + FLAGS_prediction_trajectory_time_resolution, &points); + } auto end_time2 = std::chrono::system_clock::now(); std::chrono::duration diff = end_time2 - end_time1; ADEBUG << " Time to draw trajectory: " << diff.count() * 1000 << " msec."; - Trajectory trajectory = GenerateTrajectory(points); trajectory.set_probability(sequence.probability()); trajectories_.push_back(std::move(trajectory)); diff --git a/modules/prediction/predictor/predictor.cc b/modules/prediction/predictor/predictor.cc index ec8e64f86d..3f6ab2a3bf 100644 --- a/modules/prediction/predictor/predictor.cc +++ b/modules/prediction/predictor/predictor.cc @@ -32,6 +32,7 @@ using apollo::common::PathPoint; using apollo::common::TrajectoryPoint; using apollo::common::math::LineSegment2d; using apollo::common::math::Vec2d; +using apollo::hdmap::LaneInfo; using apollo::planning::ADCTrajectory; const std::vector& Predictor::trajectories() { @@ -128,18 +129,21 @@ bool Predictor::TrimTrajectory( return true; } -void Predictor::DrawConstantAccelerationTrajectory( - const Obstacle& obstacle, const LaneSequence& lane_sequence, - const double total_time, const double period, - const double acceleration, - std::vector* points) { - // TODO(kechxu) implement -} - -bool Predictor::SupposedToStop(const Obstacle& obstacle, - const double stop_distance) { - // TODO(kechxu) implement - return false; +bool Predictor::SupposedToStop(const Feature& feature, + const double stop_distance, + double* acceleration) { + if (stop_distance < std::max(feature.length() * 0.5, 1.0)) { + return false; + } + if (stop_distance > FLAGS_distance_to_slow_down_at_stop_sign) { + return false; + } + double speed = feature.speed(); + *acceleration = -speed * speed / (2.0 * stop_distance); + if (*acceleration > -FLAGS_double_precision) { + return false; + } + return true; } } // namespace prediction diff --git a/modules/prediction/predictor/predictor.h b/modules/prediction/predictor/predictor.h index 44c1e8fda1..139b85e44e 100644 --- a/modules/prediction/predictor/predictor.h +++ b/modules/prediction/predictor/predictor.h @@ -105,29 +105,15 @@ class Predictor { const ADCTrajectoryContainer* adc_trajectory_container, Trajectory* trajectory); - /** - * @brief Draw constant acceleration trajectory points - * @param Obstacle - * @param Lane sequence - * @param Total prediction time - * @param Prediction period - * @param acceleration - * @param A vector of generated trajectory points - */ - void DrawConstantAccelerationTrajectory( - const Obstacle& obstacle, const LaneSequence& lane_sequence, - const double total_time, const double period, - const double acceleration, - std::vector* points); - /** * @brief Determine if an obstacle is supposed to stop within a distance - * @param The specific obstacle + * @param The latest feature of obstacle * @param The distance to stop + * @param The output param of acceleration * @return If the obstacle is supposed to stop within a distance */ - bool SupposedToStop(const Obstacle& obstacle, - const double stop_distance); + bool SupposedToStop(const Feature& feature, const double stop_distance, + double* acceleration); protected: std::vector trajectories_; diff --git a/modules/prediction/predictor/sequence/sequence_predictor.cc b/modules/prediction/predictor/sequence/sequence_predictor.cc index 8b96af3af0..56bffcf5a4 100644 --- a/modules/prediction/predictor/sequence/sequence_predictor.cc +++ b/modules/prediction/predictor/sequence/sequence_predictor.cc @@ -261,5 +261,73 @@ bool SequencePredictor::LaneChangeWithMaxProb(const LaneChangeType& type, return false; } +void SequencePredictor::DrawConstantAccelerationTrajectory( + const Obstacle& obstacle, const LaneSequence& lane_sequence, + const double total_time, const double period, + const double acceleration, + std::vector* points) { + const Feature& feature = obstacle.latest_feature(); + if (!feature.has_position() || !feature.has_velocity() || + !feature.position().has_x() || !feature.position().has_y()) { + AERROR << "Obstacle [" << obstacle.id() + << " is missing position or velocity"; + return; + } + + Eigen::Vector2d position(feature.position().x(), feature.position().y()); + double speed = feature.speed(); + + int lane_segment_index = 0; + std::string lane_id = + lane_sequence.lane_segment(lane_segment_index).lane_id(); + std::shared_ptr lane_info = PredictionMap::LaneById(lane_id); + double lane_s = 0.0; + double lane_l = 0.0; + if (!PredictionMap::GetProjection(position, lane_info, &lane_s, &lane_l)) { + AERROR << "Failed in getting lane s and lane l"; + return; + } + size_t total_num = static_cast(total_time / 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 (!PredictionMap::SmoothPointFromLane(lane_id, lane_s, lane_l, &point, + &theta)) { + AERROR << "Unable to get smooth point from lane [" << lane_id + << "] with s [" << lane_s << "] and l [" << lane_l << "]"; + break; + } + TrajectoryPoint trajectory_point; + PathPoint path_point; + path_point.set_x(point.x()); + path_point.set_y(point.y()); + path_point.set_z(0.0); + path_point.set_theta(theta); + path_point.set_lane_id(lane_id); + trajectory_point.mutable_path_point()->CopyFrom(path_point); + trajectory_point.set_v(speed); + trajectory_point.set_a(0.0); + trajectory_point.set_relative_time(relative_time); + points->emplace_back(std::move(trajectory_point)); + + if (speed < FLAGS_double_precision) { + continue; + } + + lane_s += speed * period + 0.5 * acceleration * period * period; + speed += acceleration * period; + + while (lane_s > PredictionMap::LaneById(lane_id)->total_length() && + lane_segment_index + 1 < lane_sequence.lane_segment_size()) { + lane_segment_index += 1; + lane_s = lane_s - PredictionMap::LaneById(lane_id)->total_length(); + lane_id = lane_sequence.lane_segment(lane_segment_index).lane_id(); + } + + lane_l *= FLAGS_go_approach_rate; + } +} + } // namespace prediction } // namespace apollo diff --git a/modules/prediction/predictor/sequence/sequence_predictor.h b/modules/prediction/predictor/sequence/sequence_predictor.h index 1c37e18889..f97a6b740d 100644 --- a/modules/prediction/predictor/sequence/sequence_predictor.h +++ b/modules/prediction/predictor/sequence/sequence_predictor.h @@ -91,6 +91,21 @@ class SequencePredictor : public Predictor { */ double GetLaneChangeDistanceWithADC(const LaneSequence& lane_sequence); + /** + * @brief Draw constant acceleration trajectory points + * @param Obstacle + * @param Lane sequence + * @param Total prediction time + * @param Prediction period + * @param acceleration + * @param A vector of generated trajectory points + */ + void DrawConstantAccelerationTrajectory( + const Obstacle& obstacle, const LaneSequence& lane_sequence, + const double total_time, const double period, + const double acceleration, + std::vector* points); + /** * @brief Clear private members */ -- GitLab