obstacle.h

浏览该文件的文档.

/******************************************************************************
 * 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 <list>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
 
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/common_msgs/perception_msgs/perception_obstacle.pb.h"
#include "modules/common_msgs/planning_msgs/decision.pb.h"
#include "modules/common_msgs/planning_msgs/sl_boundary.pb.h"
#include "modules/common_msgs/prediction_msgs/prediction_obstacle.pb.h"
 
#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/planning/planning_base/common/indexed_list.h"
#include "modules/planning/planning_base/common/speed/st_boundary.h"
#include "modules/planning/planning_base/common/util/print_debug_info.h"
#include "modules/planning/planning_base/reference_line/reference_line.h"
 
namespace apollo {
namespace planning {
 
class Obstacle {
 public:
  Obstacle() = default;
  Obstacle(const std::string& id,
           const perception::PerceptionObstacle& perception_obstacle,
           const prediction::ObstaclePriority::Priority& obstacle_priority,
           const bool is_static);
  Obstacle(const std::string& id,
           const perception::PerceptionObstacle& perception_obstacle,
           const prediction::Trajectory& trajectory,
           const prediction::ObstaclePriority::Priority& obstacle_priority,
           const bool is_static);
 
  const std::string& Id() const { return id_; }
  void SetId(const std::string& id) { id_ = id; }
 
  double speed() const { return speed_; }
 
  int32_t PerceptionId() const { return perception_id_; }
 
  bool IsStatic() const { return is_static_; }
  bool IsVirtual() const { return is_virtual_; }
 
  common::TrajectoryPoint GetPointAtTime(const double time) const;
 
  common::math::Box2d GetBoundingBox(
      const common::TrajectoryPoint& point) const;
 
  const common::math::Box2d& PerceptionBoundingBox() const {
    return perception_bounding_box_;
  }
  const common::math::Polygon2d& PerceptionPolygon() const {
    return perception_polygon_;
  }
  const prediction::Trajectory& Trajectory() const { return trajectory_; }
 
  bool HasTrajectory() const {
    return !(trajectory_.trajectory_point().empty());
  }
 
  const perception::PerceptionObstacle& Perception() const {
    return perception_obstacle_;
  }
 
  static std::list<std::unique_ptr<Obstacle>> CreateObstacles(
      const prediction::PredictionObstacles& predictions);
 
  static std::unique_ptr<Obstacle> CreateStaticVirtualObstacles(
      const std::string& id, const common::math::Box2d& obstacle_box);
 
  static bool IsValidPerceptionObstacle(
      const perception::PerceptionObstacle& obstacle);
 
  static bool IsValidTrajectoryPoint(const common::TrajectoryPoint& point);
 
  inline bool IsCautionLevelObstacle() const {
    return is_caution_level_obstacle_;
  }
 
  // const Obstacle* obstacle() const;
 
  const ObjectDecisionType& LateralDecision() const;
 
  const ObjectDecisionType& LongitudinalDecision() const;
 
  std::string DebugString() const;
 
  void PrintPolygonCurve() const;
 
  const SLBoundary& PerceptionSLBoundary() const;
 
  const STBoundary& reference_line_st_boundary() const;
 
  const STBoundary& path_st_boundary() const;
 
  const std::vector<std::string>& decider_tags() const;
 
  const std::vector<ObjectDecisionType>& decisions() const;
 
  void AddLongitudinalDecision(const std::string& decider_tag,
                               const ObjectDecisionType& decision);
 
  void AddLateralDecision(const std::string& decider_tag,
                          const ObjectDecisionType& decision);
  bool HasLateralDecision() const;
 
  void set_path_st_boundary(const STBoundary& boundary);
 
  bool is_path_st_boundary_initialized() const {
    return path_st_boundary_initialized_;
  }
 
  void SetStBoundaryType(const STBoundary::BoundaryType type);
 
  void EraseStBoundary();
 
  void EraseDecision();
 
  void SetReferenceLineStBoundary(const STBoundary& boundary);
 
  void SetReferenceLineStBoundaryType(const STBoundary::BoundaryType type);
 
  void EraseReferenceLineStBoundary();
 
  bool HasLongitudinalDecision() const;
 
  bool HasNonIgnoreDecision() const;
 
  double MinRadiusStopDistance(const common::VehicleParam& vehicle_param) const;
 
  bool IsIgnore() const;
  bool IsLongitudinalIgnore() const;
  bool IsLateralIgnore() const;
 
  void BuildReferenceLineStBoundary(const ReferenceLine& reference_line,
                                    const double adc_start_s);
 
  void SetPerceptionSlBoundary(const SLBoundary& sl_boundary);
 
  static bool IsLongitudinalDecision(const ObjectDecisionType& decision);
 
  static bool IsLateralDecision(const ObjectDecisionType& decision);
 
  void SetBlockingObstacle(bool blocking) { is_blocking_obstacle_ = blocking; }
  bool IsBlockingObstacle() const { return is_blocking_obstacle_; }
 
  /*
   * @brief IsLaneBlocking is only meaningful when IsStatic() == true.
   */
  bool IsLaneBlocking() const { return is_lane_blocking_; }
  void CheckLaneBlocking(const ReferenceLine& reference_line);
  bool IsLaneChangeBlocking() const { return is_lane_change_blocking_; }
  void SetLaneChangeBlocking(const bool is_distance_clear);
  common::math::Polygon2d GetObstacleTrajectoryPolygon(
      const common::TrajectoryPoint& point) const;
 
 private:
  FRIEND_TEST(MergeLongitudinalDecision, AllDecisions);
  static ObjectDecisionType MergeLongitudinalDecision(
      const ObjectDecisionType& lhs, const ObjectDecisionType& rhs);
  FRIEND_TEST(MergeLateralDecision, AllDecisions);
  static ObjectDecisionType MergeLateralDecision(const ObjectDecisionType& lhs,
                                                 const ObjectDecisionType& rhs);
 
  bool BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
                                 const double adc_start_s,
                                 STBoundary* const st_boundary);
  bool IsValidObstacle(
      const perception::PerceptionObstacle& perception_obstacle);
 
 private:
  std::string id_;
  int32_t perception_id_ = 0;
  bool is_static_ = false;
  bool is_virtual_ = false;
  double speed_ = 0.0;
 
  bool path_st_boundary_initialized_ = false;
 
  prediction::Trajectory trajectory_;
  perception::PerceptionObstacle perception_obstacle_;
  common::math::Box2d perception_bounding_box_;
  common::math::Polygon2d perception_polygon_;
 
  std::vector<ObjectDecisionType> decisions_;
  std::vector<std::string> decider_tags_;
  SLBoundary sl_boundary_;
 
  STBoundary reference_line_st_boundary_;
  STBoundary path_st_boundary_;
 
  ObjectDecisionType lateral_decision_;
  ObjectDecisionType longitudinal_decision_;
 
  // for keep_clear usage only
  bool is_blocking_obstacle_ = false;
 
  bool is_lane_blocking_ = false;
 
  bool is_lane_change_blocking_ = false;
 
  bool is_caution_level_obstacle_ = false;
 
  double min_radius_stop_distance_ = -1.0;
 
  struct ObjectTagCaseHash {
    size_t operator()(
        const planning::ObjectDecisionType::ObjectTagCase tag) const {
      return static_cast<size_t>(tag);
    }
  };
 
  static const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
                                  ObjectTagCaseHash>
      s_lateral_decision_safety_sorter_;
  static const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
                                  ObjectTagCaseHash>
      s_longitudinal_decision_safety_sorter_;
};
 
typedef IndexedList<std::string, Obstacle> IndexedObstacles;
typedef ThreadSafeIndexedList<std::string, Obstacle> ThreadSafeIndexedObstacles;
 
}  // namespace planning
}  // namespace apollo