st_obstacles_processor.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2019 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 <string>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/status/status.h"
#include "modules/planning/common/history.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/path/path_data.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/speed/st_boundary.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/proto/decision.pb.h"
#include "modules/planning/reference_line/reference_line.h"

namespace apollo {
namespace planning {

constexpr double kADCSafetyLBuffer = 0.1;
constexpr double kSIgnoreThreshold = 0.01;
constexpr double kTIgnoreThreshold = 0.1;
constexpr double kOvertakenObsCautionTime = 0.5;

class STObstaclesProcessor {
 public:
  STObstaclesProcessor() {}

  void Init(const double planning_distance, const double planning_time,
            const PathData& path_data, PathDecision* const path_decision,
            History* const history);

  virtual ~STObstaclesProcessor() = default;

  common::Status MapObstaclesToSTBoundaries(PathDecision* const path_decision);

  std::unordered_map<std::string, STBoundary> GetAllSTBoundaries();

  bool GetSBoundsFromDecisions(
      double t,
      std::vector<std::pair<double, double>>* const available_s_bounds,
      std::vector<
          std::vector<std::pair<std::string, ObjectDecisionType>>>* const
          available_obs_decisions);

  bool GetLimitingSpeedInfo(
      double t, std::pair<double, double>* const limiting_speed_info);

  void SetObstacleDecision(const std::string& obs_id,
                           const ObjectDecisionType& obs_decision);

  void SetObstacleDecision(
      const std::vector<std::pair<std::string, ObjectDecisionType>>&
          obstacle_decisions);

 private:
  bool ComputeObstacleSTBoundary(const Obstacle& obstacle,
                                 std::vector<STPoint>* const lower_points,
                                 std::vector<STPoint>* const upper_points,
                                 bool* const is_caution_obstacle,
                                 double* const obs_caution_end_t);

  bool GetOverlappingS(const std::vector<common::PathPoint>& adc_path_points,
                       const common::math::Box2d& obstacle_instance,
                       const double adc_l_buffer,
                       std::pair<double, double>* const overlapping_s);

  int GetSBoundingPathPointIndex(
      const std::vector<common::PathPoint>& adc_path_points,
      const common::math::Box2d& obstacle_instance, const double s_thresh,
      const bool is_before, const int start_idx, const int end_idx);

  bool IsPathPointAwayFromObstacle(const common::PathPoint& path_point,
                                   const common::PathPoint& direction_point,
                                   const common::math::Box2d& obs_box,
                                   const double s_thresh, const bool is_before);

  bool IsADCOverlappingWithObstacle(const common::PathPoint& adc_path_point,
                                    const common::math::Box2d& obs_box,
                                    const double l_buffer) const;

  std::vector<std::pair<double, double>> FindSGaps(
      const std::vector<std::tuple<int, double, double, double, std::string>>&
          obstacle_t_edges,
      double s_min, double s_max);

  ObjectDecisionType DetermineObstacleDecision(const double obs_s_min,
                                               const double obs_s_max,
                                               const double s) const;

  bool IsSWithinADCLowRoadRightSegment(const double s) const;

 private:
  double planning_time_;
  double planning_distance_;
  PathData path_data_;
  common::VehicleParam vehicle_param_;
  double adc_path_init_s_;
  PathDecision* path_decision_;

  // A vector of sorted obstacle's t-edges:
  //  (is_starting_t, t, s_min, s_max, obs_id).
  std::vector<std::tuple<int, double, double, double, std::string>>
      obs_t_edges_;
  int obs_t_edges_idx_;

  std::unordered_map<std::string, STBoundary> obs_id_to_st_boundary_;
  std::unordered_map<std::string, ObjectDecisionType> obs_id_to_decision_;

  std::vector<std::tuple<std::string, STBoundary, Obstacle*>>
      candidate_clear_zones_;

  std::unordered_map<std::string, STBoundary>
      obs_id_to_alternative_st_boundary_;

  std::vector<std::pair<double, double>> adc_low_road_right_segments_;

  History* history_ = nullptr;
};

}  // namespace planning
}  // namespace apollo