Prediction: add lon acc cost into interaction predictor

modules/prediction/common/prediction_gflags.cc

@@ -256,6 +256,8 @@ DEFINE_bool(use_bell_curve_for_cost_function, false,
 // interaction predictor
 DEFINE_double(collision_cost_time_resolution, 1.0,
               "The time resolution used to compute the collision cost");
+DEFINE_double(longitudinal_acceleration_cost_weight, 0.0,
+              "The weight of longitudinal acceleration cost");
 DEFINE_double(centripedal_acceleration_cost_weight, 0.0,
               "The weight of the cost related to centripedal acceleration");
 DEFINE_double(collision_cost_weight, 1.0,

modules/prediction/common/prediction_gflags.h

@@ -150,6 +150,7 @@ DECLARE_bool(use_bell_curve_for_cost_function);
 
 // interaction predictor
 DECLARE_double(collision_cost_time_resolution);
+DECLARE_double(longitudinal_acceleration_cost_weight);
 DECLARE_double(centripedal_acceleration_cost_weight);
 DECLARE_double(collision_cost_weight);
 DECLARE_double(collision_cost_exp_coefficient);

modules/prediction/predictor/interaction/interaction_predictor.cc

@@ -235,21 +235,26 @@ bool InteractionPredictor::DrawTrajectory(
 double InteractionPredictor::ComputeTrajectoryCost(const Obstacle& obstacle,
     const LaneSequence& lane_sequence, const double acceleration) {
   CHECK_GT(obstacle.history_size(), 0);
-  double centri_acc_weight = FLAGS_centripedal_acceleration_cost_weight;
-  double collision_weight = FLAGS_collision_cost_weight;
   double speed = obstacle.latest_feature().speed();
   double total_cost = 0.0;
+  double lon_acc_cost = LongitudinalAccelerationCost(acceleration);
+  total_cost += FLAGS_longitudinal_acceleration_cost_weight * lon_acc_cost;
   double centri_acc_cost =
       CentripetalAccelerationCost(lane_sequence, speed, acceleration);
-  total_cost += centri_acc_weight * centri_acc_cost;
+  total_cost += FLAGS_centripedal_acceleration_cost_weight * centri_acc_cost;
   if (LowerRightOfWayThanEgo(obstacle, lane_sequence)) {
     double collision_cost =
         CollisionWithEgoVehicleCost(lane_sequence, speed, acceleration);
-    total_cost += collision_weight * collision_cost;
+    total_cost += FLAGS_collision_cost_weight * collision_cost;
   }
   return total_cost;
 }
 
+double InteractionPredictor::LongitudinalAccelerationCost(
+    const double acceleration) {
+  return acceleration * acceleration;
+}
+
 double InteractionPredictor::CentripetalAccelerationCost(
     const LaneSequence& lane_sequence,
     const double speed, const double acceleration) {

modules/prediction/predictor/interaction/interaction_predictor.h

@@ -61,6 +61,8 @@ class InteractionPredictor : public SequencePredictor {
                                const LaneSequence& lane_sequence,
                                const double acceleration);
 
+  double LongitudinalAccelerationCost(const double acceleration);
+
   double CentripetalAccelerationCost(const LaneSequence& lane_sequence,
                                      const double speed,
                                      const double acceleration);