Planning: [lattice] add lat comfort cost

modules/planning/common/planning_gflags.cc

@@ -405,6 +405,7 @@ DEFINE_double(weight_lon_jerk, 1.0, "Weight of longitudinal jerk cost");
 DEFINE_double(weight_lon_collision, 2.0,
               "Weight of logitudinal collision cost");
 DEFINE_double(weight_lat_offset, 2.0, "Weight of lateral offset cost");
+DEFINE_double(weight_lat_comfort, 10.0, "Weight of lateral comfort cost");
 DEFINE_double(weight_same_side_offset, 1.0,
               "Weight of same side lateral offset cost");
 DEFINE_double(weight_opposite_side_offset, 10.0,

modules/planning/common/planning_gflags.h

@@ -213,6 +213,7 @@ DECLARE_double(weight_lon_travel);
 DECLARE_double(weight_lon_jerk);
 DECLARE_double(weight_lon_collision);
 DECLARE_double(weight_lat_offset);
+DECLARE_double(weight_lat_comfort);
 DECLARE_double(weight_same_side_offset);
 DECLARE_double(weight_opposite_side_offset);
 DECLARE_double(weight_dist_travelled);

modules/planning/lattice/trajectory_generator/trajectory_evaluator.cc

@@ -126,6 +126,7 @@ double TrajectoryEvaluator::Evaluate(
   // 2. Cost of logitudinal jerk
   // 3. Cost of logitudinal collision
   // 4. Cost of lateral offsets
+  // 5. Cost of lateral comfort
 
   // Longitudinal costs
   double lon_travel_cost = LonObjectiveCost(lon_trajectory, planning_target);
@@ -140,6 +141,7 @@ double TrajectoryEvaluator::Evaluate(
 
   // Lateral costs
   double lat_offset_cost = LatOffsetCost(lat_trajectory, s_values);
+  double lat_comfort_cost = LatComfortCost(lon_trajectory, lat_trajectory);
 
   if (cost_components) {
     cost_components->push_back(lon_travel_cost);
@@ -151,7 +153,8 @@ double TrajectoryEvaluator::Evaluate(
   return lon_travel_cost * FLAGS_weight_lon_travel +
          lon_jerk_cost * FLAGS_weight_lon_jerk +
          lon_collision_cost * FLAGS_weight_lon_collision +
-         lat_offset_cost * FLAGS_weight_lat_offset;
+         lat_offset_cost * FLAGS_weight_lat_offset +
+         lat_comfort_cost * FLAGS_weight_lat_comfort;
 }
 
 double TrajectoryEvaluator::LatOffsetCost(
@@ -174,6 +177,23 @@ double TrajectoryEvaluator::LatOffsetCost(
   return cost_sqr_sum / (cost_abs_sum + FLAGS_lattice_epsilon);
 }
 
+double TrajectoryEvaluator::LatComfortCost(
+    const std::shared_ptr<Trajectory1d>& lon_trajectory,
+    const std::shared_ptr<Trajectory1d>& lat_trajectory) const {
+  double cost_sqr_sum = 0.0;
+  double cost_abs_sum = 0.0;
+  for (double t = 0.0; t < FLAGS_trajectory_time_length;
+       t += FLAGS_trajectory_time_resolution) {
+    double s = lon_trajectory->Evaluate(0, t);
+    double cost = lat_trajectory->Evaluate(1, s) *
+                  lon_trajectory->Evaluate(1, t) /
+                  FLAGS_default_cruise_speed;
+    cost_sqr_sum += cost * cost;
+    cost_abs_sum += std::abs(cost);
+  }
+  return cost_sqr_sum / (cost_abs_sum + FLAGS_lattice_epsilon);
+}
+
 double TrajectoryEvaluator::LonComfortCost(
     const std::shared_ptr<Trajectory1d>& lon_trajectory) const {
   double cost_sqr_sum = 0.0;

modules/planning/lattice/trajectory_generator/trajectory_evaluator.h

@@ -82,6 +82,9 @@ class TrajectoryEvaluator {
   double LatOffsetCost(const std::shared_ptr<Curve1d>& lat_trajectory,
                        const std::vector<double>& s_values) const;
 
+  double LatComfortCost(const std::shared_ptr<Curve1d>& lon_trajectory,
+                        const std::shared_ptr<Curve1d>& lat_trajectory) const;
+
   double LonComfortCost(const std::shared_ptr<Curve1d>& lon_trajectory) const;
 
   double LonCollisionCost(const std::shared_ptr<Curve1d>& lon_trajectory) const;