navi_speed_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 <string>
#include <vector>

#include "gtest/gtest.h"
#include "modules/common/status/status.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/navi/decider/navi_obstacle_decider.h"
#include "modules/planning/navi/decider/navi_speed_ts_graph.h"
#include "modules/planning/navi/decider/navi_task.h"
#include "modules/planning/proto/planning_config.pb.h"

namespace apollo {
namespace planning {

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

  bool Init(const PlanningConfig& config) override;

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

 private:
  apollo::common::Status MakeSpeedDecision(
      double start_v, double start_a, double start_da,
      const std::vector<common::PathPoint>& path_points,
      const std::vector<const Obstacle*>& obstacles,
      const std::function<const Obstacle*(const std::string& id)>&
          find_obstacle,
      SpeedData* const speed_data);

  apollo::common::Status AddPerceptionRangeConstraints();

  apollo::common::Status AddObstaclesConstraints(
      double vehicle_speed, double path_length,
      const std::vector<common::PathPoint>& path_points,
      const std::vector<const Obstacle*>& obstacles,
      const std::function<const Obstacle*(const std::string& id)>&
          find_obstacle);

  apollo::common::Status AddTrafficDecisionConstraints();

  apollo::common::Status AddCentricAccelerationConstraints(
      const std::vector<common::PathPoint>& path_points);

  apollo::common::Status AddConfiguredConstraints();

  void RecordDebugInfo(const SpeedData& speed_data);

 private:
  double preferred_speed_;
  double max_speed_;
  double preferred_accel_;
  double preferred_decel_;
  double preferred_jerk_;
  double max_accel_;
  double max_decel_;
  double obstacle_buffer_;
  double safe_distance_base_;
  double safe_distance_ratio_;
  double following_accel_ratio_;
  double soft_centric_accel_limit_;
  double hard_centric_accel_limit_;
  double hard_speed_limit_;
  double hard_accel_limit_;
  bool enable_safe_path_;
  bool enable_planning_start_point_;
  bool enable_accel_auto_compensation_;
  double kappa_preview_;
  double kappa_threshold_;

  NaviObstacleDecider obstacle_decider_;
  NaviSpeedTsGraph ts_graph_;

  double prev_v_ = 0.0;
  double accel_compensation_ratio_ = 1.0;
  double decel_compensation_ratio_ = 1.0;

  FRIEND_TEST(NaviSpeedDeciderTest, CreateSpeedData);
  FRIEND_TEST(NaviSpeedDeciderTest, CreateSpeedDataForStaticObstacle);
  FRIEND_TEST(NaviSpeedDeciderTest, CreateSpeedDataForObstacles);
  FRIEND_TEST(NaviSpeedDeciderTest, CreateSpeedDataForCurve);
};

}  // namespace planning
}  // namespace apollo