From b5f59cb77088c221b634d36e481157a3925c67a6 Mon Sep 17 00:00:00 2001
From: Calvin Miao <jhmiao@gmail.com>
Date: Mon, 24 Jul 2017 10:40:51 -0700
Subject: [PATCH] Added is on lane check for obstacle

---
 modules/prediction/container/obstacles/obstacle.cc | 12 ++++++++++++
 modules/prediction/container/obstacles/obstacle.h  |  2 ++
 modules/prediction/predictor/predictor_manager.cc  |  9 +++++++--
 3 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/modules/prediction/container/obstacles/obstacle.cc b/modules/prediction/container/obstacles/obstacle.cc
index 3d055da9e9..73d3f14c22 100644
--- a/modules/prediction/container/obstacles/obstacle.cc
+++ b/modules/prediction/container/obstacles/obstacle.cc
@@ -112,10 +112,22 @@ size_t Obstacle::history_size() const {
 
 const KalmanFilter<double, 4, 2, 0>& Obstacle::kf_lane_tracker(
     const std::string& lane_id) {
+  std::lock_guard<std::mutex> lock(mutex_);
   CHECK(kf_lane_trackers_.find(lane_id) != kf_lane_trackers_.end());
   return kf_lane_trackers_[lane_id];
 }
 
+bool Obstacle::IsOnLane() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (feature_history_.size() > 0) {
+    if (feature_history_.front().has_lane() &&
+        feature_history_.front().lane().has_lane_feature()) {
+      return true;
+    }
+  }
+  return false;
+}
+
 void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
                       const double timestamp) {
   std::lock_guard<std::mutex> lock(mutex_);
diff --git a/modules/prediction/container/obstacles/obstacle.h b/modules/prediction/container/obstacles/obstacle.h
index 67db9317d3..4692cb95a5 100644
--- a/modules/prediction/container/obstacles/obstacle.h
+++ b/modules/prediction/container/obstacles/obstacle.h
@@ -66,6 +66,8 @@ class Obstacle {
 
   // TODO(author) void SetLaneGraphFeature(ObstacleClusters* p_cluster);
 
+  bool IsOnLane();
+
  private:
   apollo::common::ErrorCode SetId(
       const apollo::perception::PerceptionObstacle& perception_obstacle,
diff --git a/modules/prediction/predictor/predictor_manager.cc b/modules/prediction/predictor/predictor_manager.cc
index 4d262f8cb0..d70a569857 100644
--- a/modules/prediction/predictor/predictor_manager.cc
+++ b/modules/prediction/predictor/predictor_manager.cc
@@ -45,9 +45,15 @@ void PredictorManager::Run(
   for (const auto& perception_obstacle :
       perception_obstacles.perception_obstacle()) {
     int id = perception_obstacle.id();
+    Obstacle* obstacle = container->GetObstacle(id);
+    CHECK_NOTNULL(obstacle);
     switch (perception_obstacle.type()) {
       case PerceptionObstacle::VEHICLE: {
-        predictor = GetPredictor(ObstacleConf::LANE_SEQUENCE_PREDICTOR);
+        if (obstacle->IsOnLane()) {
+          predictor = GetPredictor(ObstacleConf::LANE_SEQUENCE_PREDICTOR);
+        } else {
+          predictor = GetPredictor(ObstacleConf::FREE_MOVE_PREDICTOR);
+        }
         break;
       }
       case PerceptionObstacle::PEDESTRIAN: {
@@ -60,7 +66,6 @@ void PredictorManager::Run(
       }
     }
     if (predictor != nullptr) {
-      Obstacle* obstacle = container->GetObstacle(id);
       predictor->Predict(obstacle);
       PredictionObstacle prediction_obstacle;
       prediction_obstacle.CopyFrom(predictor->prediction_obstacle());
-- 
GitLab