Prediction: Implement build lane features in cruise scenario

modules/prediction/scenario/analyzer/scenario_analyzer.cc

@@ -15,8 +15,10 @@
  *****************************************************************************/
 
 #include "modules/prediction/scenario/analyzer/scenario_analyzer.h"
+
+#include <string>
+
 #include "modules/prediction/common/prediction_gflags.h"
-#include "modules/prediction/scenario/scenario_features/cruise_scenario_features.h"
 
 namespace apollo {
 namespace prediction {
@@ -32,6 +34,13 @@ void ScenarioAnalyzer::Analyze(
   } else if (environment_features.has_ego_lane()) {
     scenario_.set_type(Scenario::CRUISE);
   }
+
+  if (scenario_.type() == Scenario::CRUISE) {
+    std::shared_ptr<CruiseScenarioFeatures> cruise_scenario_features =
+        std::make_shared<CruiseScenarioFeatures>();
+    cruise_scenario_features->BuildCruiseScenarioFeatures(environment_features);
+    scenario_features_ = cruise_scenario_features;
+  }
 }
 
 const Scenario& ScenarioAnalyzer::scenario() const {
@@ -43,12 +52,5 @@ ScenarioAnalyzer::GetScenarioFeatures() {
   return scenario_features_;
 }
 
-void ScenarioAnalyzer::BuildCruiseScenarioFeatures(
-    const EnvironmentFeatures& environment_features) {
-  // TODO(kechxu) implement
-  std::shared_ptr<CruiseScenarioFeatures> cruise_scenario_features;
-  scenario_features_ = cruise_scenario_features;
-}
-
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/scenario/analyzer/scenario_analyzer.h

@@ -22,6 +22,7 @@
 #include "modules/prediction/proto/scenario.pb.h"
 #include "modules/prediction/common/environment_features.h"
 #include "modules/prediction/scenario/scenario_features/scenario_features.h"
+#include "modules/prediction/scenario/scenario_features/cruise_scenario_features.h"
 
 namespace apollo {
 namespace prediction {
@@ -38,10 +39,6 @@ class ScenarioAnalyzer {
 
   std::shared_ptr<ScenarioFeatures> GetScenarioFeatures();
 
- private:
-  void BuildCruiseScenarioFeatures(
-      const EnvironmentFeatures& environment_features);
-
  private:
   Scenario scenario_;
 

modules/prediction/scenario/feature_extractor/feature_extractor.cc

@@ -87,7 +87,7 @@ void FeatureExtractor::ExtractEgoLaneFeatures(const LaneInfoPtr& ptr_ego_lane,
     const common::math::Vec2d& ego_position) {
 
   if (ptr_ego_lane == nullptr) {
-    AERROR << "Ego vehicle is not on any lane.";
+    ADEBUG << "Ego vehicle is not on any lane.";
     return;
   }
   double curr_lane_s = 0.0;

modules/prediction/scenario/scenario_features/BUILD

@@ -9,6 +9,9 @@ cc_library(
     hdrs = ["cruise_scenario_features.h", 
             "scenario_features.h"],
     deps = [
+        "//modules/prediction/common:prediction_gflags",
+        "//modules/prediction/common:prediction_map",
+        "//modules/prediction/common:environment_features",
         "//framework:cybertron",
     ],
 )

modules/prediction/scenario/scenario_features/cruise_scenario_features.cc

@@ -14,15 +14,22 @@
  * limitations under the License.
  *****************************************************************************/
 
-/**
- * @file
- */
-
 #include "modules/prediction/scenario/scenario_features/cruise_scenario_features.h"
 
+#include <queue>
+#include <memory>
+#include <utility>
+
+#include "modules/prediction/common/prediction_gflags.h"
+#include "modules/prediction/common/prediction_map.h"
+
 namespace apollo {
 namespace prediction {
 
+using apollo::hdmap::LaneInfo;
+using ConstLaneInfoPtr = std::shared_ptr<const LaneInfo>;
+using apollo::hdmap::Lane;
+
 CruiseScenarioFeatures::CruiseScenarioFeatures() {}
 
 CruiseScenarioFeatures::~CruiseScenarioFeatures() {}
@@ -35,5 +42,57 @@ void CruiseScenarioFeatures::InsertLaneOfInterest(const std::string lane_id) {
   lane_ids_of_interest_.insert(lane_id);
 }
 
+void CruiseScenarioFeatures::BuildCruiseScenarioFeatures(
+    const EnvironmentFeatures& environment_features) {
+  if (environment_features.has_ego_lane()) {
+    auto ego_lane = environment_features.GetEgoLane();
+    const std::string& ego_lane_id = ego_lane.first;
+    double ego_lane_s = ego_lane.second;
+    SearchAndInsertLanes(ego_lane_id, ego_lane_s, 50.0);
+  }
+  if (environment_features.has_left_neighbor_lane()) {
+    auto left_lane = environment_features.GetLeftNeighborLane();
+    const std::string& left_lane_id = left_lane.first;
+    double left_lane_s = left_lane.second;
+    SearchAndInsertLanes(left_lane_id, left_lane_s, 50.0);
+  }
+  if (environment_features.has_right_neighbor_lane()) {
+    auto right_lane = environment_features.GetRightNeighborLane();
+    const std::string& right_lane_id = right_lane.first;
+    double right_lane_s = right_lane.second;
+    SearchAndInsertLanes(right_lane_id, right_lane_s, 50.0);
+  }
+}
+
+void CruiseScenarioFeatures::SearchAndInsertLanes(
+    const std::string& start_lane_id, const double start_lane_s,
+    const double range) {
+  ConstLaneInfoPtr start_lane_info = PredictionMap::LaneById(start_lane_id);
+  double start_lane_length = start_lane_info->total_length();
+  double start_accumulated_s = start_lane_length - start_lane_s;
+  std::queue<std::pair<ConstLaneInfoPtr, double>> lane_queue;
+  lane_queue.emplace(start_lane_info, start_accumulated_s);
+
+  while (!lane_queue.empty()) {
+    ConstLaneInfoPtr lane_info = lane_queue.front().first;
+    double accumulated_s = lane_queue.front().second;
+    lane_queue.pop();
+    InsertLaneOfInterest(lane_info->id().id());
+    const Lane& lane = lane_info->lane();
+    for (const auto prede_lane_id : lane.predecessor_id()) {
+      InsertLaneOfInterest(prede_lane_id.id());
+    }
+    if (accumulated_s > range) {
+      continue;
+    }
+    for (const auto succ_lane_id : lane.successor_id()) {
+      ConstLaneInfoPtr succ_lane_info =
+          PredictionMap::LaneById(succ_lane_id.id());
+      double succ_lane_length = succ_lane_info->total_length();
+      lane_queue.emplace(succ_lane_info, accumulated_s + succ_lane_length);
+    }
+  }
+}
+
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/scenario/scenario_features/cruise_scenario_features.h

@@ -25,6 +25,7 @@
 #include <unordered_set>
 
 #include "modules/prediction/scenario/scenario_features/scenario_features.h"
+#include "modules/prediction/common/environment_features.h"
 
 namespace apollo {
 namespace prediction {
@@ -39,6 +40,14 @@ class CruiseScenarioFeatures : public ScenarioFeatures {
 
   void InsertLaneOfInterest(const std::string lane_id);
 
+  void BuildCruiseScenarioFeatures(
+      const EnvironmentFeatures& environment_features);
+
+ private:
+  void SearchAndInsertLanes(
+      const std::string& lane_id, const double start_lane_s,
+      const double range);
+
  private:
   std::unordered_set<std::string> lane_ids_of_interest_;
 };