Planning :Autotuning add obstacle feature as well as feature flatten function

modules/planning/tuning/speed_model/autotuning_speed_mlp_model.cc

@@ -19,6 +19,18 @@
 namespace apollo {
 namespace planning {
 
+/**
+  * @brief: max considerred obstacle range
+  */
+namespace {
+constexpr double kMaxFollow = 100.0;
+constexpr double kMaxOvertake = 100.0;
+constexpr double kMaxStop = 60.0;
+constexpr double kMaxNudge = 60.0;
+constexpr double kMaxNudgeLateralDistance = 10.0;
+constexpr double kMaxSidePassDistance = 100.0;
+}
+
 common::Status AutotuningSpeedMLPModel::SetParams() {
   mlp_model_.reset(new AutotuningMLPModel());
   feature_builder_.reset(new AutotuningSpeedFeatureBuilder());
@@ -35,5 +47,138 @@ double AutotuningSpeedMLPModel::Evaluate(
   return 0.0;
 }
 
+void AutotuningSpeedMLPModel::FlattenFeatures(
+    const autotuning::TrajectoryFeature& feature,
+    Eigen::MatrixXf* const flat_feature) const {
+  int row_count = feature.point_feature_size();
+  int col_count = 21;
+  flat_feature->resize(row_count, col_count);
+
+  for (int i = 0; i < row_count; ++i) {
+    FlattenFeatures(feature.point_feature(i).speed_input_feature(), i,
+                    flat_feature);
+  }
+}
+
+void AutotuningSpeedMLPModel::FlattenFeatures(
+    const autotuning::SpeedPointwiseFeature& speed_point_feature, const int row,
+    Eigen::MatrixXf* const flat_feature) const {
+  double v = speed_point_feature.v();
+  double acc = speed_point_feature.acc();
+  double jerk = speed_point_feature.jerk();
+  double speed_limit = speed_point_feature.speed_limit();
+
+  double lateral_acc = speed_point_feature.lateral_acc();
+  // has collision , collision = 1, else 0
+  double collision = 0.0;
+  // obstacle feature, looping to find among all obstacle
+  double follow_distance = kMaxFollow;
+  double follow_v_rel = 0.0;
+  double overtake_distance = kMaxOvertake;
+  double overtake_v_rel = 0.0;
+  double stop_distance = kMaxStop;
+  double stop_v_rel = 0.0;
+  double virtual_distance = kMaxStop;
+  double virtual_v_rel = 0.0;
+  double nudge_distance = kMaxNudge;
+  double nudge_v_rel = 0.0;
+  double nudge_lateral_distance = kMaxNudgeLateralDistance;
+  double sidepass_front_v_rel = 0.0;
+  double sidepass_front_distance = kMaxSidePassDistance;
+  double sidepass_rear_v_rel = 0.0;
+  double sidepass_rear_distance = kMaxSidePassDistance;
+
+  // looping obstacles to find the value minimize the distance
+  for (int i = 0; i < speed_point_feature.follow_obs_feature_size(); ++i) {
+    const auto& follow_obs_feature = speed_point_feature.follow_obs_feature(i);
+    if (follow_distance > follow_obs_feature.longitudinal_distance()) {
+      follow_distance = follow_obs_feature.longitudinal_distance();
+      follow_v_rel = follow_obs_feature.relative_v();
+    }
+  }
+  // looping obstacles to find the value minimize the distance
+  for (int i = 0; i < speed_point_feature.overtake_obs_feature_size(); ++i) {
+    const auto& overtake_obs_feature =
+        speed_point_feature.overtake_obs_feature(i);
+    if (overtake_distance > overtake_obs_feature.longitudinal_distance()) {
+      overtake_distance = overtake_obs_feature.longitudinal_distance();
+      overtake_v_rel = overtake_obs_feature.relative_v();
+    }
+  }
+  // looping obstacles to find the value minimize the distance
+  for (int i = 0; i < speed_point_feature.stop_obs_feature_size(); ++i) {
+    const auto& stop_obs_feature = speed_point_feature.stop_obs_feature(i);
+    if (stop_distance > stop_obs_feature.longitudinal_distance()) {
+      stop_distance = stop_obs_feature.longitudinal_distance();
+      stop_v_rel = stop_obs_feature.relative_v();
+    }
+  }
+  // looping obstacles to find the value minimize the distance
+  for (int i = 0; i < speed_point_feature.virtual_obs_feature_size(); ++i) {
+    const auto& virtual_obs_feature =
+        speed_point_feature.virtual_obs_feature(i);
+    if (virtual_distance > virtual_obs_feature.longitudinal_distance()) {
+      virtual_distance = virtual_obs_feature.longitudinal_distance();
+      virtual_v_rel = virtual_obs_feature.relative_v();
+    }
+  }
+  // looping obstacles to find the value minimize the distance
+  for (int i = 0; i < speed_point_feature.nudge_obs_feature_size(); ++i) {
+    const auto& nudge_obs_feature = speed_point_feature.nudge_obs_feature(i);
+    if (nudge_distance > nudge_obs_feature.longitudinal_distance()) {
+      nudge_distance = nudge_obs_feature.longitudinal_distance();
+      nudge_v_rel = nudge_obs_feature.relative_v();
+      nudge_lateral_distance = nudge_obs_feature.lateral_distance();
+    }
+  }
+  // looping obstacles to find the value minimize the distance
+  for (int i = 0; i < speed_point_feature.sidepass_front_obs_feature_size();
+       ++i) {
+    const auto& sidepass_front_obs_feature =
+        speed_point_feature.sidepass_front_obs_feature(i);
+    if (sidepass_front_distance >
+        sidepass_front_obs_feature.longitudinal_distance()) {
+      sidepass_front_distance =
+          sidepass_front_obs_feature.longitudinal_distance();
+      sidepass_front_v_rel = sidepass_front_obs_feature.relative_v();
+    }
+  }
+  // looping obstacles to find the value minimize the distance
+  for (int i = 0; i < speed_point_feature.sidepass_rear_obs_feature_size();
+       ++i) {
+    const auto& sidepass_rear_obs_feature =
+        speed_point_feature.sidepass_rear_obs_feature(i);
+    if (sidepass_rear_distance >
+        sidepass_rear_obs_feature.longitudinal_distance()) {
+      sidepass_rear_distance =
+          sidepass_rear_obs_feature.longitudinal_distance();
+      sidepass_rear_v_rel = sidepass_rear_obs_feature.relative_v();
+    }
+  }
+
+  (*flat_feature)(row, 0) = v;
+  (*flat_feature)(row, 1) = speed_limit;
+  (*flat_feature)(row, 2) = acc;
+  (*flat_feature)(row, 3) = jerk;
+  (*flat_feature)(row, 4) = lateral_acc;
+  (*flat_feature)(row, 5) = collision;
+  (*flat_feature)(row, 6) = follow_distance;
+  (*flat_feature)(row, 7) = follow_v_rel;
+  (*flat_feature)(row, 8) = overtake_distance;
+  (*flat_feature)(row, 9) = overtake_v_rel;
+  (*flat_feature)(row, 10) = nudge_distance;
+  (*flat_feature)(row, 11) = nudge_v_rel;
+  (*flat_feature)(row, 12) = nudge_lateral_distance;
+  (*flat_feature)(row, 13) = stop_distance;
+  (*flat_feature)(row, 14) = stop_v_rel;
+  (*flat_feature)(row, 15) = virtual_distance;
+  (*flat_feature)(row, 16) = virtual_v_rel;
+  (*flat_feature)(row, 17) = sidepass_front_distance;
+  (*flat_feature)(row, 18) = sidepass_front_v_rel;
+  (*flat_feature)(row, 19) = sidepass_rear_distance;
+  (*flat_feature)(row, 20) = sidepass_rear_v_rel;
+  return;
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/tuning/speed_model/autotuning_speed_mlp_model.h

@@ -48,6 +48,19 @@ class AutotuningSpeedMLPModel : public AutotuningBaseModel {
    */
   double Evaluate(const autotuning::TrajectoryPointwiseFeature& point_feature)
       const override;
+
+ private:
+  /**
+   * [FlattenFeatures description]
+   * @param feature      [model input proto]
+   * @param flat_feature [eigen matrix, row: time, col: traj point feature]
+   */
+  void FlattenFeatures(const autotuning::TrajectoryFeature& feature,
+                       Eigen::MatrixXf* const flat_feature) const;
+
+  void FlattenFeatures(
+      const autotuning::SpeedPointwiseFeature& speed_point_feature,
+      const int row, Eigen::MatrixXf* const flat_feature) const;
 };
 
 }  // namespace planning