planning: filter out obstacle based on perception information

modules/planning/common/path_obstacle.cc

@@ -64,22 +64,8 @@ PathObstacle::PathObstacle(const Obstacle* obstacle) : obstacle_(obstacle) {
   id_ = obstacle_->Id();
 }
 
-bool PathObstacle::Init(const ReferenceLine& reference_line,
-                        double adc_start_s) {
-  if (!reference_line.GetSLBoundary(obstacle_->PerceptionBoundingBox(),
-                                    &perception_sl_boundary_)) {
-    AERROR << "Failed to get sl boundary for obstacle: " << id_;
-    return false;
-  }
-
-  if (perception_sl_boundary_.start_s() < 0 ||
-      perception_sl_boundary_.end_s() > reference_line.Length()) {
-    return true;
-  }
-
-  // TODO(All): reduce the calculation time of BuildStBoundary
-  BuildStBoundary(reference_line, adc_start_s);
-  return true;
+void PathObstacle::SetPerceptionSlBoundary(const SLBoundary& sl_boundary) {
+  perception_sl_boundary_ = sl_boundary;
 }
 
 void PathObstacle::BuildStBoundary(const ReferenceLine& reference_line,

modules/planning/common/path_obstacle.h

@@ -65,8 +65,6 @@ class PathObstacle {
   PathObstacle() = default;
   explicit PathObstacle(const Obstacle* obstacle);
 
-  bool Init(const ReferenceLine& reference_line, const double adc_start_s);
-
   const std::string& Id() const;
 
   const Obstacle* obstacle() const;
@@ -120,6 +118,11 @@ class PathObstacle {
   bool IsLongitudinalIgnore() const;
   bool IsLateralIgnore() const;
 
+  void BuildStBoundary(const ReferenceLine& reference_line,
+                       const double adc_start_s);
+
+  void SetPerceptionSlBoundary(const SLBoundary& sl_boundary);
+
  private:
   /**
    * @brief check if a ObjectDecisionType is a lateral decision.
@@ -140,9 +143,6 @@ class PathObstacle {
   static ObjectDecisionType MergeLateralDecision(const ObjectDecisionType& lhs,
                                                  const ObjectDecisionType& rhs);
 
-  void BuildStBoundary(const ReferenceLine& reference_line,
-                       const double adc_start_s);
-
   bool BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
                                  const double adc_start_s,
                                  StBoundary* const st_boundary);

modules/planning/common/reference_line_info.cc

@@ -85,6 +85,7 @@ bool ReferenceLineInfo::Init() {
     AERROR << "Ego vehicle is too far away from reference line.";
     return false;
   }
+  is_on_reference_line_ = reference_line_.IsOnRoad(adc_sl_boundary_);
   return true;
 }
 
@@ -148,12 +149,29 @@ void ReferenceLineInfo::SetTrajectory(const DiscretizedTrajectory& trajectory) {
 }
 
 PathObstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) {
-  auto path_obstacle = CreatePathObstacle(obstacle);
-  if (!path_obstacle) {
-    AERROR << "Failed to create path obstacle for " << obstacle->Id();
-    return nullptr;
+  auto path_obstacle_ptr =
+      std::unique_ptr<PathObstacle>(new PathObstacle(obstacle));
+  auto* path_obstacle = path_decision_.AddPathObstacle(*path_obstacle_ptr);
+
+  SLBoundary perception_sl;
+  if (!reference_line_.GetSLBoundary(obstacle->PerceptionBoundingBox(),
+                                     &perception_sl)) {
+    AERROR << "Failed to get sl boundary for obstacle: " << obstacle->Id();
+    return path_obstacle;
   }
-  return path_decision_.AddPathObstacle(*path_obstacle);
+  path_obstacle->SetPerceptionSlBoundary(perception_sl);
+
+  if (IsUnrelaventObstacle(path_obstacle)) {
+    ObjectDecisionType ignore;
+    ignore.mutable_ignore();
+    path_decision_.AddLateralDecision("reference_line_filter", obstacle->Id(),
+                                      ignore);
+    path_decision_.AddLongitudinalDecision("reference_line_filter",
+                                           obstacle->Id(), ignore);
+  } else {
+    path_obstacle->BuildStBoundary(reference_line_, adc_sl_boundary_.start_s());
+  }
+  return path_obstacle;
 }
 
 bool ReferenceLineInfo::AddObstacles(
@@ -167,16 +185,19 @@ bool ReferenceLineInfo::AddObstacles(
   return true;
 }
 
-std::unique_ptr<PathObstacle> ReferenceLineInfo::CreatePathObstacle(
-    const Obstacle* obstacle) {
-  auto path_obstacle =
-      std::unique_ptr<PathObstacle>(new PathObstacle(obstacle));
-  if (!path_obstacle->Init(reference_line_, adc_sl_boundary_.end_s())) {
-    AERROR << "Failed to create perception sl boundary for obstacle "
-           << obstacle->Id();
-    return nullptr;
+bool ReferenceLineInfo::IsUnrelaventObstacle(PathObstacle* path_obstacle) {
+  // if adc is on the road, and obstacle behind adc, ignore
+  if (path_obstacle->perception_sl_boundary().end_s() >
+      reference_line_.Length()) {
+    return true;
   }
-  return path_obstacle;
+  if (is_on_reference_line_ &&
+      path_obstacle->perception_sl_boundary().end_s() <
+          adc_sl_boundary_.end_s() &&
+      reference_line_.IsOnRoad(path_obstacle->perception_sl_boundary())) {
+    return true;
+  }
+  return false;
 }
 
 const DiscretizedTrajectory& ReferenceLineInfo::trajectory() const {

modules/planning/common/reference_line_info.h

@@ -118,8 +118,7 @@ class ReferenceLineInfo {
  private:
   void ExportTurnSignal(common::VehicleSignal* signal) const;
 
-  std::unique_ptr<PathObstacle> CreatePathObstacle(const Obstacle* obstacle);
-  bool InitPerceptionSLBoundary(PathObstacle* path_obstacle);
+  bool IsUnrelaventObstacle(PathObstacle* path_obstacle);
 
   void MakeDecision(DecisionResult* decision_result) const;
   int MakeMainStopDecision(DecisionResult* decision_result) const;
@@ -152,6 +151,8 @@ class ReferenceLineInfo {
 
   hdmap::RouteSegments lanes_;
 
+  bool is_on_reference_line_ = false;
+
   ADCTrajectory::RightOfWayStatus status_ = ADCTrajectory::UNPROTECTED;
 
   DISALLOW_COPY_AND_ASSIGN(ReferenceLineInfo);

modules/planning/reference_line/reference_line.cc

@@ -383,6 +383,18 @@ bool ReferenceLine::IsOnRoad(const common::math::Vec2d& vec2d_point) const {
   return IsOnRoad(sl_point);
 }
 
+bool ReferenceLine::IsOnRoad(const SLBoundary& sl_boundary) const {
+  if (sl_boundary.end_s() < 0 || sl_boundary.start_s() > Length()) {
+    return false;
+  }
+  double middle_s = (sl_boundary.start_s() + sl_boundary.end_s()) / 2.0;
+  double left_width = 0.0;
+  double right_width = 0.0;
+  map_path_.GetWidth(middle_s, &left_width, &right_width);
+  return sl_boundary.start_l() >= -right_width &&
+         sl_boundary.end_l() <= left_width;
+}
+
 bool ReferenceLine::IsBlockRoad(const common::math::Box2d& box2d,
                                 double gap) const {
   return map_path_.OverlapWith(box2d, gap);

modules/planning/reference_line/reference_line.h

@@ -96,6 +96,7 @@ class ReferenceLine {
                     double* const right_width) const;
   bool IsOnRoad(const common::SLPoint& sl_point) const;
   bool IsOnRoad(const common::math::Vec2d& vec2d_point) const;
+  bool IsOnRoad(const SLBoundary& sl_boundary) const;
 
   /**
    * @brief Check if a box is blocking the road surface. The crieria is to check

modules/planning/tasks/traffic_decider/backside_vehicle.cc

@@ -55,8 +55,8 @@ void BacksideVehicle::MakeLaneKeepingObstacleDecision(
       continue;
     }
 
-    if (path_obstacle->perception_sl_boundary().start_s()
-        < adc_sl_boundary.end_s()) {
+    if (path_obstacle->perception_sl_boundary().start_s() <
+        adc_sl_boundary.end_s()) {
       path_decision->AddLongitudinalDecision(rule_id, path_obstacle->Id(),
                                              ignore);
       path_decision->AddLateralDecision(rule_id, path_obstacle->Id(), ignore);