navi_path_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 <vector>

#include "gflags/gflags.h"
#include "gtest/gtest_prod.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/common/status/status.h"
#include "modules/planning/navi/decider/navi_obstacle_decider.h"
#include "modules/planning/navi/decider/navi_task.h"
#include "modules/planning/proto/planning_config.pb.h"
#include "modules/planning/proto/task_config.pb.h"
#include "modules/planning/reference_line/reference_line.h"

namespace apollo {
namespace planning {

class NaviPathDecider : public NaviTask {
 public:
  NaviPathDecider();
  virtual ~NaviPathDecider() = default;

  bool Init(const PlanningConfig &config) override;

  apollo::common::Status Execute(
      Frame *frame, ReferenceLineInfo *reference_line_info) override;

 private:
  apollo::common::Status Process(const ReferenceLine &reference_line,
                                 const common::TrajectoryPoint &init_point,
                                 const std::vector<const Obstacle *> &obstacles,
                                 PathDecision *const path_decision,
                                 PathData *const path_data);

  bool GetBasicPathData(const ReferenceLine &reference_line,
                        std::vector<common::PathPoint> *const path_points);

  void MoveToDestLane(const double dest_ref_line_y,
                      std::vector<common::PathPoint> *const path_points);

  void KeepLane(const double dest_ref_line_y,
                std::vector<common::PathPoint> *const path_points);

  void RecordDebugInfo(const PathData &path_data);

  bool IsSafeChangeLane(const ReferenceLine &reference_line,
                        const PathDecision &path_decision);

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

 private:
  double max_keep_lane_distance_ = 0.0;
  double min_keep_lane_offset_ = 0.0;
  double max_keep_lane_shift_y_ = 0.0;
  double keep_lane_shift_compensation_ = 0.0;
  double move_dest_lane_compensation_ = 0.0;
  uint32_t start_plan_point_from_ = 0;
  std::map<double, double> move_dest_lane_config_talbe_;
  std::vector<double> max_speed_levels_;

  double start_plan_v_ = 0.0;
  double start_plan_a_ = 0.0;
  apollo::common::PathPoint start_plan_point_;

  std::string cur_reference_line_lane_id_;
  std::map<std::string, bool> last_lane_id_to_nudge_flag_;
  NaviObstacleDecider obstacle_decider_;
  common::VehicleState vehicle_state_;
  NaviPathDeciderConfig config_;

  FRIEND_TEST(NaviPathDeciderTest, MoveToDestLane);
  FRIEND_TEST(NaviPathDeciderTest, KeepLane);
};

}  // namespace planning
}  // namespace apollo