Prediction: fixed trajectory heading calculation

modules/prediction/common/prediction_util.cc

@@ -33,13 +33,9 @@ double Normalize(const double value, const double mean, const double std) {
   return (value - mean) / (std + eps);
 }
 
-double Sigmoid(const double value) {
-  return 1 / (1 + std::exp(-1.0 * value));
-}
+double Sigmoid(const double value) { return 1 / (1 + std::exp(-1.0 * value)); }
 
-double Relu(const double value) {
-  return (value > 0.0) ? value : 0.0;
-}
+double Relu(const double value) { return (value > 0.0) ? value : 0.0; }
 
 int SolveQuadraticEquation(const std::vector<double>& coefficients,
                            std::pair<double, double>* roots) {
@@ -179,19 +175,6 @@ void GenerateLaneSequenceTrajectoryPoints(
       break;
     }
 
-    if (points->size() > 0) {
-      PathPoint* prev_point = points->back().mutable_path_point();
-      double x_diff = point.x() - prev_point->x();
-      double y_diff = point.y() - prev_point->y();
-      if (std::fabs(x_diff) > std::numeric_limits<double>::epsilon() ||
-          std::fabs(y_diff) > std::numeric_limits<double>::epsilon()) {
-        theta = std::atan2(y_diff, x_diff);
-        prev_point->set_theta(theta);
-      } else {
-        theta = prev_point->theta();
-      }
-    }
-
     // update state
     if (lane_speed <= 0.0) {
       ADEBUG << "Non-positive lane_speed tacked : " << lane_speed;

modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc

@@ -65,7 +65,10 @@ void LaneSequencePredictor::Predict(Obstacle* obstacle) {
     return;
   }
 
-  std::string lane_id = feature.lane().lane_feature().lane_id();
+  std::string lane_id = "";
+  if (feature.lane().has_lane_feature()) {
+    lane_id = feature.lane().lane_feature().lane_id();
+  }
   int num_lane_sequence = feature.lane().lane_graph().lane_sequence_size();
   std::vector<bool> enable_lane_sequence(num_lane_sequence, true);
   FilterLaneSequences(feature.lane().lane_graph(), lane_id,

modules/prediction/predictor/move_sequence/move_sequence_predictor.cc

@@ -195,9 +195,9 @@ void MoveSequencePredictor::DrawManeuverTrajectoryPoints(
     position[0] = feature.t_position().x();
     position[1] = feature.t_position().y();
   }
-  double time_to_lane_center = std::max(
-      FLAGS_default_time_to_lane_center,
-      ComputeTimeToLaneCenterByVelocity(obstacle, lane_sequence));
+  double time_to_lane_center =
+      std::max(FLAGS_default_time_to_lane_center,
+               ComputeTimeToLaneCenterByVelocity(obstacle, lane_sequence));
 
   std::array<double, 6> lateral_coeffs;
   std::array<double, 5> longitudinal_coeffs;
@@ -222,6 +222,7 @@ void MoveSequencePredictor::DrawManeuverTrajectoryPoints(
 
   size_t total_num = static_cast<size_t>(total_time / period);
   size_t num_to_center = static_cast<size_t>(time_to_lane_center / period);
+  AERROR << "Obstacle: " << obstacle.id();
   for (size_t i = 0; i < total_num; ++i) {
     double relative_time = static_cast<double>(i) * period;
     Eigen::Vector2d point;
@@ -248,20 +249,6 @@ void MoveSequencePredictor::DrawManeuverTrajectoryPoints(
     }
 
     prev_lane_l = lane_l;
-
-    if (points->size() > 0) {
-      PathPoint* prev_point = points->back().mutable_path_point();
-      double x_diff = point.x() - prev_point->x();
-      double y_diff = point.y() - prev_point->y();
-      if (std::fabs(x_diff) > std::numeric_limits<double>::epsilon() ||
-          std::fabs(y_diff) > std::numeric_limits<double>::epsilon()) {
-        theta = std::atan2(y_diff, x_diff);
-        prev_point->set_theta(theta);
-      } else {
-        theta = prev_point->theta();
-      }
-    }
-
     double vs =
         EvaluateLongitudinalPolynomial(longitudinal_coeffs, relative_time, 1);
     double as =

modules/prediction/predictor/sequence/sequence_predictor.cc

@@ -88,7 +88,8 @@ void SequencePredictor::FilterLaneSequences(
     // The obstacle has interference with ADC within a small distance
     if (distance < FLAGS_lane_change_dist &&
         (lane_change_type[i] == LaneChangeType::LEFT ||
-         lane_change_type[i] == LaneChangeType::RIGHT)) {
+         lane_change_type[i] == LaneChangeType::RIGHT ||
+         lane_change_type[i] == LaneChangeType::ONTO_LANE)) {
       (*enable_lane_sequence)[i] = false;
       ADEBUG << "Filter trajectory [" << ToString(sequence)
              << "] due to small distance " << distance << ".";
@@ -163,6 +164,10 @@ SequencePredictor::LaneChangeType SequencePredictor::GetLaneChangeType(
     const std::string& lane_id, const LaneSequence& lane_sequence) {
   PredictionMap* map = PredictionMap::instance();
 
+  if (lane_id.empty()) {
+    return LaneChangeType::ONTO_LANE;
+  }
+
   std::string lane_change_id = lane_sequence.lane_segment(0).lane_id();
   if (lane_id == lane_change_id) {
     return LaneChangeType::STRAIGHT;

modules/prediction/predictor/sequence/sequence_predictor.h

@@ -37,6 +37,7 @@ class SequencePredictor : public Predictor {
     LEFT,
     RIGHT,
     STRAIGHT,
+    ONTO_LANE,
     INVALID,
   };