apollo::planning::NaviPlanning类 参考

#include <navi_planning.h>

类 apollo::planning::NaviPlanning 继承关系图:

apollo::planning::NaviPlanning 的协作图:

Public 成员函数
	NaviPlanning (const std::shared_ptr< DependencyInjector > &injector)
virtual	~NaviPlanning ()
std::string	Name () const override
	Planning algorithm name.
apollo::common::Status	Init (const PlanningConfig &config) override
	module initialization function
void	RunOnce (const LocalView &local_view, ADCTrajectory *const trajectory_pb) override
	main logic of the planning module, runs periodically triggered by timer.
apollo::common::Status	Plan (const double current_time_stamp, const std::vector< common::TrajectoryPoint > &stitching_trajectory, ADCTrajectory *const trajectory) override
	Plan the trajectory given current vehicle state
Public 成员函数 继承自 apollo::planning::PlanningBase
	PlanningBase ()=delete
	PlanningBase (const std::shared_ptr< DependencyInjector > &injector)
virtual	~PlanningBase ()
bool	IsPlanningFinished (const ADCTrajectory::TrajectoryType &current_trajectory_type) const
	Check if vehicle reaches the end point of the RoutingRequest.
bool	GenerateWidthOfLane (const Vec2d &current_location, Vec2d &left_point, Vec2d &right_point)

额外继承的成员函数
Protected 成员函数 继承自 apollo::planning::PlanningBase
virtual void	FillPlanningPb (const double timestamp, ADCTrajectory *const trajectory_pb)
void	LoadPlanner ()
Protected 属性 继承自 apollo::planning::PlanningBase
LocalView	local_view_
const hdmap::HDMap *	hdmap_ = nullptr
double	start_time_ = 0.0
size_t	seq_num_ = 0
PlanningConfig	config_
TrafficDecider	traffic_decider_
std::unique_ptr< Frame >	frame_
std::shared_ptr< Planner >	planner_
std::unique_ptr< PublishableTrajectory >	last_publishable_trajectory_
std::shared_ptr< DependencyInjector >	injector_

详细描述

在文件 navi_planning.h 第 42 行定义.

构造及析构函数说明

NaviPlanning()

apollo::planning::NaviPlanning::NaviPlanning ( const std::shared_ptr< DependencyInjector > &  injector )

inlineexplicit

在文件 navi_planning.h 第 44 行定义.

      : PlanningBase(injector) {}

~NaviPlanning()

apollo::planning::NaviPlanning::~NaviPlanning ( )

virtual

在文件 navi_planning.cc 第 51 行定义.

                            {
  last_publishable_trajectory_.reset(nullptr);
  frame_.reset(nullptr);
  injector_->frame_history()->Clear();
  injector_->history()->Clear();
  injector_->planning_context()->mutable_planning_status()->Clear();
}

成员函数说明

Init()

Status apollo::planning::NaviPlanning::Init ( const PlanningConfig &  config )

overridevirtual

module initialization function

返回

initialization status

重载 apollo::planning::PlanningBase .

在文件 navi_planning.cc 第 61 行定义.

                                                      {
  if (!CheckPlanningConfig(config)) {
    return Status(ErrorCode::PLANNING_ERROR,
                  "planning config error: " + config.DebugString());
  }
 
  PlanningBase::Init(config);
 
  // clear planning history
  injector_->history()->Clear();
 
  // clear planning status
  injector_->planning_context()->mutable_planning_status()->Clear();
 
  LoadPlanner();
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }
 
  traffic_decider_.Init(injector_);
 
  return planner_->Init(injector_, FLAGS_planner_config_path);
}

Name()

std::string apollo::planning::NaviPlanning::Name ( ) const

overridevirtual

Planning algorithm name.

实现了 apollo::planning::PlanningBase.

在文件 navi_planning.cc 第 59 行定义.

{ return "navi_planning"; }

Plan()

Status apollo::planning::NaviPlanning::Plan ( const double  current_time_stamp, const std::vector< common::TrajectoryPoint > &  stitching_trajectory, ADCTrajectory *const  trajectory  )

overridevirtual

Plan the trajectory given current vehicle state

if (FLAGS_enable_stitch_last_trajectory) { last_publishable_trajectory_->PrependTrajectoryPoints( std::vector<TrajectoryPoint>(stitching_trajectory.begin(), stitching_trajectory.end() - 1)); }

实现了 apollo::planning::PlanningBase.

在文件 navi_planning.cc 第 468 行定义.

                                        {
  auto* ptr_debug = trajectory_pb->mutable_debug();
  if (FLAGS_enable_record_debug) {
    ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom(
        stitching_trajectory.back());
  }
 
  auto status =
      planner_->Plan(stitching_trajectory.back(), frame_.get(), trajectory_pb);
 
  ExportReferenceLineDebug(ptr_debug);
 
  const auto* best_ref_info = frame_->FindDriveReferenceLineInfo();
  if (!best_ref_info) {
    const std::string msg = "planner failed to make a driving plan";
    AERROR << msg;
    if (last_publishable_trajectory_) {
      last_publishable_trajectory_->Clear();
    }
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  ptr_debug->MergeFrom(best_ref_info->debug());
  trajectory_pb->mutable_latency_stats()->MergeFrom(
      best_ref_info->latency_stats());
  // set right of way status
  trajectory_pb->set_right_of_way_status(best_ref_info->GetRightOfWayStatus());
  for (const auto& id : best_ref_info->TargetLaneId()) {
    trajectory_pb->add_lane_id()->CopyFrom(id);
  }
 
  best_ref_info->ExportDecision(trajectory_pb->mutable_decision(),
                                injector_->planning_context());
 
  // Add debug information.
  if (FLAGS_enable_record_debug) {
    auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
    reference_line->set_name("planning_reference_line");
    const auto& reference_points =
        best_ref_info->reference_line().reference_points();
    double s = 0.0;
    double prev_x = 0.0;
    double prev_y = 0.0;
    bool empty_path = true;
    for (const auto& reference_point : reference_points) {
      auto* path_point = reference_line->add_path_point();
      path_point->set_x(reference_point.x());
      path_point->set_y(reference_point.y());
      path_point->set_theta(reference_point.heading());
      path_point->set_kappa(reference_point.kappa());
      path_point->set_dkappa(reference_point.dkappa());
      if (empty_path) {
        path_point->set_s(0.0);
        empty_path = false;
      } else {
        double dx = reference_point.x() - prev_x;
        double dy = reference_point.y() - prev_y;
        s += std::hypot(dx, dy);
        path_point->set_s(s);
      }
      prev_x = reference_point.x();
      prev_y = reference_point.y();
    }
  }
 
  last_publishable_trajectory_.reset(new PublishableTrajectory(
      current_time_stamp, best_ref_info->trajectory()));
 
  ADEBUG << "current_time_stamp: " << current_time_stamp;
 
  // Navi Planner doesn't need to stitch the last path planning
  // trajectory.Otherwise, it will cause the Dreamview planning track to display
  // flashing or bouncing
  // TODO(Yifei): remove this if navi-planner doesn't need stitching
  for (size_t i = 0; i < last_publishable_trajectory_->NumOfPoints(); ++i) {
    if (last_publishable_trajectory_->TrajectoryPointAt(i).relative_time() >
        FLAGS_trajectory_time_high_density_period) {
      break;
    }
    ADEBUG << last_publishable_trajectory_->TrajectoryPointAt(i)
                  .ShortDebugString();
  }
 
  last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb);
 
  best_ref_info->ExportEngageAdvice(trajectory_pb->mutable_engage_advice(),
                                    injector_->planning_context());
 
  return status;
}

RunOnce()

void apollo::planning::NaviPlanning::RunOnce ( const LocalView &  local_view, ADCTrajectory *const  trajectory_pb  )

overridevirtual

main logic of the planning module, runs periodically triggered by timer.

实现了 apollo::planning::PlanningBase.

在文件 navi_planning.cc 第 113 行定义.

                                                               {
  local_view_ = local_view;
  const double start_timestamp = Clock::NowInSeconds();
 
  // recreate reference line provider in every cycle
  hdmap_ = HDMapUtil::BaseMapPtr(*local_view.relative_map);
  const ReferenceLineConfig* reference_line_config = nullptr;
  if (config_.has_reference_line_config()) {
    reference_line_config = &config_.reference_line_config();
  }
  // Prefer "std::make_unique" to direct use of "new".
  // Refer to "https://herbsutter.com/gotw/_102/" for details.
  reference_line_provider_ = std::make_unique<ReferenceLineProvider>(
      injector_->vehicle_state(), reference_line_config,
      local_view_.relative_map);
 
  // localization
  ADEBUG << "Get localization: "
         << local_view_.localization_estimate->DebugString();
 
  // chassis
  ADEBUG << "Get chassis: " << local_view_.chassis->DebugString();
 
  Status status = injector_->vehicle_state()->Update(
      *local_view_.localization_estimate, *local_view_.chassis);
 
  auto vehicle_config =
      ComputeVehicleConfigFromLocalization(*local_view_.localization_estimate);
 
  if (last_vehicle_config_.is_valid_ && vehicle_config.is_valid_) {
    auto x_diff_map = vehicle_config.x_ - last_vehicle_config_.x_;
    auto y_diff_map = vehicle_config.y_ - last_vehicle_config_.y_;
 
    auto cos_map_veh = std::cos(last_vehicle_config_.theta_);
    auto sin_map_veh = std::sin(last_vehicle_config_.theta_);
 
    auto x_diff_veh = cos_map_veh * x_diff_map + sin_map_veh * y_diff_map;
    auto y_diff_veh = -sin_map_veh * x_diff_map + cos_map_veh * y_diff_map;
 
    auto theta_diff = vehicle_config.theta_ - last_vehicle_config_.theta_;
 
    TrajectoryStitcher::TransformLastPublishedTrajectory(
        x_diff_veh, y_diff_veh, theta_diff, last_publishable_trajectory_.get());
  }
  last_vehicle_config_ = vehicle_config;
 
  VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state();
 
  // estimate (x, y) at current timestamp
  // This estimate is only valid if the current time and vehicle state timestamp
  // differs only a small amount (20ms). When the different is too large, the
  // estimation is invalid.
  DCHECK_GE(start_timestamp, vehicle_state.timestamp());
  if (start_timestamp - vehicle_state.timestamp() <
      FLAGS_message_latency_threshold) {
    auto future_xy = injector_->vehicle_state()->EstimateFuturePosition(
        start_timestamp - vehicle_state.timestamp());
    vehicle_state.set_x(future_xy.x());
    vehicle_state.set_y(future_xy.y());
    vehicle_state.set_timestamp(start_timestamp);
  }
 
  auto* not_ready = trajectory_pb->mutable_decision()
                        ->mutable_main_decision()
                        ->mutable_not_ready();
 
  if (!status.ok() || !util::IsVehicleStateValid(vehicle_state)) {
    const std::string msg = "Update VehicleStateProvider failed";
    AERROR << msg;
    not_ready->set_reason(msg);
    status.Save(trajectory_pb->mutable_header()->mutable_status());
    // TODO(all): integrate reverse gear
    trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    FillPlanningPb(start_timestamp, trajectory_pb);
    return;
  }
 
  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
 
  std::vector<TrajectoryPoint> stitching_trajectory;
  std::string replan_reason;
  stitching_trajectory = TrajectoryStitcher::ComputeStitchingTrajectory(
      *(local_view_.chassis), vehicle_state, start_timestamp,
      planning_cycle_time, FLAGS_trajectory_stitching_preserved_length, true,
      last_publishable_trajectory_.get(), &replan_reason,
      *local_view_.control_interactive_msg);
 
  const uint32_t frame_num = static_cast<uint32_t>(seq_num_++);
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
 
  if (!frame_) {
    const std::string msg = "Failed to init frame";
    AERROR << msg;
    not_ready->set_reason(msg);
    status.Save(trajectory_pb->mutable_header()->mutable_status());
    // TODO(all): integrate reverse gear
    trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    FillPlanningPb(start_timestamp, trajectory_pb);
    return;
  }
 
  injector_->ego_info()->Update(stitching_trajectory.back(), vehicle_state);
 
  if (FLAGS_enable_record_debug) {
    frame_->RecordInputDebug(trajectory_pb->mutable_debug());
  }
  trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms(
      Clock::NowInSeconds() - start_timestamp);
  if (!status.ok()) {
    AERROR << status.ToString();
    if (FLAGS_publish_estop) {
      // Because the function "Control::ProduceControlCommand()" checks the
      // "estop" signal with the following line (Line 170 in control.cc):
      // estop_ = estop_ || trajectory_.estop().is_estop();
      // we should add more information to ensure the estop being triggered.
      ADCTrajectory estop_trajectory;
      EStop* estop = estop_trajectory.mutable_estop();
      estop->set_is_estop(true);
      estop->set_reason(status.error_message());
      status.Save(estop_trajectory.mutable_header()->mutable_status());
      // TODO(all): integrate reverse gear
      trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
      FillPlanningPb(start_timestamp, &estop_trajectory);
    } else {
      trajectory_pb->mutable_decision()
          ->mutable_main_decision()
          ->mutable_not_ready()
          ->set_reason(status.ToString());
      status.Save(trajectory_pb->mutable_header()->mutable_status());
      // TODO(all): integrate reverse gear
      trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
      FillPlanningPb(start_timestamp, trajectory_pb);
    }
 
    frame_->set_current_frame_planned_trajectory(*trajectory_pb);
    auto seq_num = frame_->SequenceNum();
    injector_->frame_history()->Add(seq_num, std::move(frame_));
 
    return;
  }
 
  // Use planning pad message to make driving decisions
  if (FLAGS_enable_planning_pad_msg) {
    const auto& pad_msg_driving_action = frame_->GetPadMsgDrivingAction();
    ProcessPadMsg(pad_msg_driving_action);
  }
 
  for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {
    auto traffic_status =
        traffic_decider_.Execute(frame_.get(), &ref_line_info);
    if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {
      ref_line_info.SetDrivable(false);
      AWARN << "Reference line " << ref_line_info.Lanes().Id()
            << " traffic decider failed";
      continue;
    }
  }
 
  status = Plan(start_timestamp, stitching_trajectory, trajectory_pb);
 
  const auto time_diff_ms = (Clock::NowInSeconds() - start_timestamp) * 1000;
  ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";
 
  trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: "
         << trajectory_pb->latency_stats().DebugString();
 
  auto* ref_line_task =
      trajectory_pb->mutable_latency_stats()->add_task_stats();
  ref_line_task->set_time_ms(reference_line_provider_->LastTimeDelay() *
                             1000.0);
  ref_line_task->set_name("ReferenceLineProvider");
 
  if (!status.ok()) {
    status.Save(trajectory_pb->mutable_header()->mutable_status());
    AERROR << "Planning failed:" << status.ToString();
    if (FLAGS_publish_estop) {
      AERROR << "Planning failed and set estop";
      // Because the function "Control::ProduceControlCommand()" checks the
      // "estop" signal with the following line (Line 170 in control.cc):
      // estop_ = estop_ || trajectory_.estop().is_estop();
      // we should add more information to ensure the estop being triggered.
      EStop* estop = trajectory_pb->mutable_estop();
      estop->set_is_estop(true);
      estop->set_reason(status.error_message());
    }
  }
 
  trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  FillPlanningPb(start_timestamp, trajectory_pb);
  ADEBUG << "Planning pb:" << trajectory_pb->header().DebugString();
 
  auto seq_num = frame_->SequenceNum();
  injector_->frame_history()->Add(seq_num, std::move(frame_));
}

---

该类的文档由以下文件生成:

• modules/planning/planning_component/navi_planning.h
• modules/planning/planning_component/navi_planning.cc