navi_obstacle_decider.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2018 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 <map>
#include <string>
#include <tuple>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/navi/decider/navi_task.h"

namespace apollo {
namespace planning {

class NaviObstacleDecider : public NaviTask {
 public:
  NaviObstacleDecider();

  virtual ~NaviObstacleDecider() = default;
  bool Init(const PlanningConfig &config) override;

  double GetNudgeDistance(
      const std::vector<const Obstacle *> &obstacles,
      const ReferenceLine &reference_line, const PathDecision &path_decision,
      const std::vector<common::PathPoint> &path_data_points,
      const common::VehicleState &vehicle_state, int *lane_obstacles_num);

  void GetUnsafeObstaclesInfo(
      const std::vector<common::PathPoint> &path_data_points,
      const std::vector<const Obstacle *> &obstacles);

  const std::vector<std::tuple<std::string, double, double>> &UnsafeObstacles();

 private:
  const apollo::common::VehicleParam &VehicleParam();

  void SetLastNudgeDistance(double dist);

  void ProcessObstacle(const std::vector<const Obstacle *> &obstacles,
                       const std::vector<common::PathPoint> &path_data_points,
                       const PathDecision &path_decision,
                       const double min_lane_width,
                       const common::VehicleState &vehicle_state);

  void AddObstacleOffsetDirection(
      const common::PathPoint &projection_point,
      const std::vector<common::PathPoint> &path_data_points,
      const Obstacle *current_obstacle, const double proj_len, double *dist);

  bool IsNeedFilterObstacle(
      const Obstacle *current_obstacle,
      const common::PathPoint &vehicle_projection_point,
      const std::vector<common::PathPoint> &path_data_points,
      const common::VehicleState &vehicle_state,
      common::PathPoint *projection_point_ptr);

  double GetMinLaneWidth(const std::vector<common::PathPoint> &path_data_points,
                         const ReferenceLine &reference_line);

  void RecordLastNudgeDistance(const double nudge_dist);

  double GetObstacleActualOffsetDistance(
      std::map<double, double>::iterator iter, const double right_nedge_lane,
      const double left_nudge_lane, int *lane_obstacles_num);
  void SmoothNudgeDistance(const common::VehicleState &vehicle_state,
                           double *nudge_dist);
  void KeepNudgePosition(const double nudge_dist, int *lane_obstacles_num);

 private:
  NaviObstacleDeciderConfig config_;
  std::map<double, double> obstacle_lat_dist_;
  std::vector<std::tuple<std::string, double, double>> unsafe_obstacle_info_;
  double last_nudge_dist_ = 0.0;
  unsigned int no_nudge_num_ = 0;
  unsigned int limit_speed_num_ = 0;
  unsigned int eliminate_clutter_num_ = 0;
  unsigned int last_lane_obstacles_num_ = 0;
  unsigned int statist_count_ = 0;
  unsigned int cycles_count_ = 0;
  bool is_obstacle_stable_ = false;
  bool keep_nudge_flag_ = false;

  FRIEND_TEST(NaviObstacleDeciderTest, ComputeNudgeDist1);
  FRIEND_TEST(NaviObstacleDeciderTest, ComputeNudgeDist2);
  FRIEND_TEST(NaviObstacleDeciderTest, ComputeNudgeDist3);
  FRIEND_TEST(NaviObstacleDeciderTest, ComputeNudgeDist4);
  FRIEND_TEST(NaviObstacleDeciderTest, GetUnsafeObstaclesID);
  // TODO(all): Add your member functions and variables.
};

inline const apollo::common::VehicleParam &NaviObstacleDecider::VehicleParam() {
  const auto &vehicle_param = apollo::common::VehicleConfigHelper::Instance()
                                  ->GetConfig()
                                  .vehicle_param();
  return vehicle_param;
}

inline const std::vector<std::tuple<std::string, double, double>>
    &NaviObstacleDecider::UnsafeObstacles() {
  return unsafe_obstacle_info_;
}

inline void NaviObstacleDecider::SetLastNudgeDistance(double dist) {
  last_nudge_dist_ = dist;
}
}  // namespace planning
}  // namespace apollo