obstacle.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <deque>
#include <list>
#include <memory>
#include <string>
#include <unordered_set>
#include <vector>

#include "modules/common/filters/digital_filter.h"
#include "modules/common/math/kalman_filter.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/prediction/common/junction_analyzer.h"
#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/container/obstacles/obstacle_clusters.h"
#include "modules/prediction/proto/feature.pb.h"
#include "modules/prediction/proto/prediction_conf.pb.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"

namespace apollo {
namespace prediction {

class Obstacle {
 public:
  static std::unique_ptr<Obstacle> Create(
      const perception::PerceptionObstacle& perception_obstacle,
      const double timestamp, const int prediction_id,
      ObstacleClusters* clusters_ptr);

  static std::unique_ptr<Obstacle> Create(const Feature& feature,
                                          ObstacleClusters* clusters_ptr);

  Obstacle() = default;

  virtual ~Obstacle() = default;

  void SetJunctionAnalyzer(JunctionAnalyzer* junction_analyzer) {
    junction_analyzer_ = junction_analyzer;
  }

  bool Insert(const perception::PerceptionObstacle& perception_obstacle,
              const double timestamp, const int prediction_id);

  bool InsertFeature(const Feature& feature);

  void ClearOldInformation();

  void TrimHistory(const size_t remain_size);

  perception::PerceptionObstacle::Type type() const;

  bool IsPedestrian() const;

  int id() const;

  double timestamp() const;

  bool ReceivedOlderMessage(const double timestamp) const;

  const Feature& feature(const size_t i) const;

  Feature* mutable_feature(const size_t i);

  const Feature& latest_feature() const;

  const Feature& earliest_feature() const;

  Feature* mutable_latest_feature();

  void SetNearbyObstacles();

  size_t history_size() const;

  bool IsStill();

  bool IsSlow();

  bool IsOnLane() const;

  bool ToIgnore();

  bool IsNearJunction();

  bool IsInJunction(const std::string& junction_id) const;

  bool IsCloseToJunctionExit() const;

  bool HasJunctionFeatureWithExits() const;

  void BuildJunctionFeature();

  void BuildLaneGraph();

  void BuildLaneGraphFromLeftToRight();

  void SetCaution();

  bool IsCaution() const;

  void SetEvaluatorType(const ObstacleConf::EvaluatorType& evaluator_type);

  void SetPredictorType(const ObstacleConf::PredictorType& predictor_type);

  const ObstacleConf& obstacle_conf() { return obstacle_conf_; }

  PredictionObstacle GeneratePredictionObstacle();

 private:
  void SetStatus(const perception::PerceptionObstacle& perception_obstacle,
                 double timestamp, Feature* feature);

  bool SetId(const perception::PerceptionObstacle& perception_obstacle,
             Feature* feature, const int prediction_id = -1);

  void SetType(const perception::PerceptionObstacle& perception_obstacle,
               Feature* feature);

  void SetIsNearJunction(
      const perception::PerceptionObstacle& perception_obstacle,
      Feature* feature);

  void SetTimestamp(const perception::PerceptionObstacle& perception_obstacle,
                    const double timestamp, Feature* feature);

  void SetPolygonPoints(
      const perception::PerceptionObstacle& perception_obstacle,
      Feature* feature);

  void SetPosition(const perception::PerceptionObstacle& perception_obstacle,
                   Feature* feature);

  void SetVelocity(const perception::PerceptionObstacle& perception_obstacle,
                   Feature* feature);

  void AdjustHeadingByLane(Feature* feature);

  void UpdateVelocity(const double theta, double* velocity_x,
                      double* velocity_y, double* velocity_heading,
                      double* speed);

  void SetAcceleration(Feature* feature);

  void SetTheta(const perception::PerceptionObstacle& perception_obstacle,
                Feature* feature);

  void SetLengthWidthHeight(
      const perception::PerceptionObstacle& perception_obstacle,
      Feature* feature);

  void UpdateLaneBelief(Feature* feature);

  void SetCurrentLanes(Feature* feature);

  void SetNearbyLanes(Feature* feature);

  void SetSurroundingLaneIds(Feature* feature, const double radius);

  void SetLaneSequenceStopSign(LaneSequence* lane_sequence_ptr);

  void SetLanePoints(Feature* feature);
  void SetLanePoints(const Feature* feature, const double lane_point_spacing,
                     const uint64_t max_num_lane_point,
                     const bool is_bidirection, LaneGraph* const lane_graph);

  void SetLaneSequencePath(LaneGraph* const lane_graph);

  void SetMotionStatus();

  void SetMotionStatusBySpeed();

  void InsertFeatureToHistory(const Feature& feature);

  void SetJunctionFeatureWithEnterLane(const std::string& enter_lane_id,
                                       Feature* const feature_ptr);

  void SetJunctionFeatureWithoutEnterLane(Feature* const feature_ptr);

  void DiscardOutdatedHistory();

  void GetNeighborLaneSegments(
      std::shared_ptr<const apollo::hdmap::LaneInfo> center_lane_info,
      bool is_left, int recursion_depth,
      std::list<std::string>* const lane_ids_ordered,
      std::unordered_set<std::string>* const existing_lane_ids);

  bool HasJunctionExitLane(
      const LaneSequence& lane_sequence,
      const std::unordered_set<std::string>& exit_lane_id_set);

  void SetClusters(ObstacleClusters* clusters_ptr);

 private:
  int id_ = FLAGS_ego_vehicle_id;

  perception::PerceptionObstacle::Type type_ =
      perception::PerceptionObstacle::UNKNOWN_UNMOVABLE;

  std::deque<Feature> feature_history_;

  std::vector<std::shared_ptr<const hdmap::LaneInfo>> current_lanes_;

  ObstacleConf obstacle_conf_;

  ObstacleClusters* clusters_ptr_ = nullptr;
  JunctionAnalyzer* junction_analyzer_ = nullptr;
};

}  // namespace prediction
}  // namespace apollo