apollo::planning::LaneFollowStage类 参考

#include <lane_follow_stage.h>

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

apollo::planning::LaneFollowStage 的协作图:

Public 成员函数
StageResult	Process (const common::TrajectoryPoint &planning_init_point, Frame *frame) override
	Each stage does its business logic inside Process function.
StageResult	PlanOnReferenceLine (const common::TrajectoryPoint &planning_start_point, Frame *frame, ReferenceLineInfo *reference_line_info)
void	PlanFallbackTrajectory (const common::TrajectoryPoint &planning_start_point, Frame *frame, ReferenceLineInfo *reference_line_info)
common::SLPoint	GetStopSL (const ObjectStop &stop_decision, const ReferenceLine &reference_line) const
void	RecordObstacleDebugInfo (ReferenceLineInfo *reference_line_info)
Public 成员函数 继承自 apollo::planning::Stage
	Stage ()
virtual bool	Init (const StagePipeline &config, const std::shared_ptr< DependencyInjector > &injector, const std::string &config_dir, void *context)
virtual	~Stage ()=default
const std::string &	Name () const
template<typename T >
T *	GetContextAs () const
const std::string &	NextStage () const

额外继承的成员函数
Protected 成员函数 继承自 apollo::planning::Stage
StageResult	ExecuteTaskOnReferenceLine (const common::TrajectoryPoint &planning_start_point, Frame *frame)
StageResult	ExecuteTaskOnReferenceLineForOnlineLearning (const common::TrajectoryPoint &planning_start_point, Frame *frame)
StageResult	ExecuteTaskOnOpenSpace (Frame *frame)
virtual StageResult	FinishScenario ()
void	RecordDebugInfo (ReferenceLineInfo *reference_line_info, const std::string &name, const double time_diff_ms)
Protected 属性 继承自 apollo::planning::Stage
std::vector< std::shared_ptr< Task > >	task_list_
std::shared_ptr< Task >	fallback_task_
std::string	next_stage_
void *	context_
std::shared_ptr< DependencyInjector >	injector_
StagePipeline	pipeline_config_

详细描述

在文件 lane_follow_stage.h 第 39 行定义.

成员函数说明

GetStopSL()

SLPoint apollo::planning::LaneFollowStage::GetStopSL	(	const ObjectStop &	stop_decision,
		const ReferenceLine &	reference_line
	)		const

在文件 lane_follow_stage.cc 第 259 行定义.

                                                                              {
  SLPoint sl_point;
  reference_line.XYToSL(stop_decision.stop_point(), &sl_point);
  return sl_point;
}

PlanFallbackTrajectory()

void apollo::planning::LaneFollowStage::PlanFallbackTrajectory	(	const common::TrajectoryPoint &	planning_start_point,
		Frame *	frame,
		ReferenceLineInfo *	reference_line_info
	)

PlanOnReferenceLine()

StageResult apollo::planning::LaneFollowStage::PlanOnReferenceLine	(	const common::TrajectoryPoint &	planning_start_point,
		Frame *	frame,
		ReferenceLineInfo *	reference_line_info
	)

在文件 lane_follow_stage.cc 第 136 行定义.

                                            {
  if (!reference_line_info->IsChangeLanePath()) {
    reference_line_info->AddCost(kStraightForwardLineCost);
  }
  ADEBUG << "planning start point:" << planning_start_point.DebugString();
  ADEBUG << "Current reference_line_info is IsChangeLanePath: "
         << reference_line_info->IsChangeLanePath();
 
  StageResult ret;
  for (auto task : task_list_) {
    const double start_timestamp = Clock::NowInSeconds();
    const auto start_planning_perf_timestamp =
        std::chrono::duration<double>(
            std::chrono::system_clock::now().time_since_epoch())
            .count();
 
    ret.SetTaskStatus(task->Execute(frame, reference_line_info));
 
    const double end_timestamp = Clock::NowInSeconds();
    const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
    ADEBUG << "after task[" << task->Name()
           << "]:" << reference_line_info->PathSpeedDebugString();
    ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";
    RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);
 
    const auto end_planning_perf_timestamp =
        std::chrono::duration<double>(
            std::chrono::system_clock::now().time_since_epoch())
            .count();
    const auto plnning_perf_ms =
        (end_planning_perf_timestamp - start_planning_perf_timestamp) * 1000;
    AINFO << "Planning Perf: task name [" << task->Name() << "], "
          << plnning_perf_ms << " ms.";
 
    if (ret.IsTaskError()) {
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.GetTaskStatus().error_message();
      break;
    }
 
    // TODO(SHU): disable reference line order changes for now
    // updated reference_line_info, because it is changed in
    // lane_change_decider by PrioritizeChangeLane().
    // reference_line_info = &frame->mutable_reference_line_info()->front();
    // ADEBUG << "Current reference_line_info is IsChangeLanePath: "
    //        << reference_line_info->IsChangeLanePath();
  }
 
  RecordObstacleDebugInfo(reference_line_info);
 
  // check path and speed results for path or speed fallback
  reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
  if (ret.IsTaskError()) {
    fallback_task_->Execute(frame, reference_line_info);
  }
 
  DiscretizedTrajectory trajectory;
  if (!reference_line_info->CombinePathAndSpeedProfile(
          planning_start_point.relative_time(),
          planning_start_point.path_point().s(), &trajectory)) {
    const std::string msg = "Fail to aggregate planning trajectory.";
    AERROR << msg;
    return ret.SetStageStatus(StageStatusType::ERROR, msg);
  }
 
  // determine if there is a destination on reference line.
  double dest_stop_s = -1.0;
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->LongitudinalDecision().has_stop() &&
        obstacle->LongitudinalDecision().stop().reason_code() ==
            STOP_REASON_DESTINATION) {
      SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                  reference_line_info->reference_line());
      dest_stop_s = dest_sl.s();
    }
  }
 
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    if (!obstacle->IsStatic()) {
      continue;
    }
    if (obstacle->LongitudinalDecision().has_stop()) {
      bool add_stop_obstacle_cost = false;
      if (dest_stop_s < 0.0) {
        add_stop_obstacle_cost = true;
      } else {
        SLPoint stop_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                    reference_line_info->reference_line());
        if (stop_sl.s() < dest_stop_s &&
            (dest_stop_s - reference_line_info->AdcSlBoundary().end_s()) <
                20.0) {
          add_stop_obstacle_cost = true;
        }
      }
      if (add_stop_obstacle_cost) {
        static constexpr double kReferenceLineStaticObsCost = 1e3;
        reference_line_info->AddCost(kReferenceLineStaticObsCost);
      }
    }
  }
 
  if (FLAGS_enable_trajectory_check) {
    if (ConstraintChecker::ValidTrajectory(trajectory) !=
        ConstraintChecker::Result::VALID) {
      const std::string msg = "Current planning trajectory is not valid.";
      AERROR << msg;
      return ret.SetStageStatus(StageStatusType::ERROR, msg);
    }
  }
 
  reference_line_info->SetTrajectory(trajectory);
  reference_line_info->SetDrivable(true);
  ret.SetStageStatus(StageStatusType::RUNNING);
  return ret;
}

Process()

StageResult apollo::planning::LaneFollowStage::Process ( const common::TrajectoryPoint &  planning_init_point, Frame *  frame  )

overridevirtual

Each stage does its business logic inside Process function.

If the stage want to transit to a different stage after finish, it should set the type of 'next_stage_'.

实现了 apollo::planning::Stage.

在文件 lane_follow_stage.cc 第 82 行定义.

                                                               {
  if (frame->reference_line_info().empty()) {
    return StageResult(StageStatusType::FINISHED);
  }
 
  bool has_drivable_reference_line = false;
 
  ADEBUG << "Number of reference lines:\t"
         << frame->mutable_reference_line_info()->size();
 
  unsigned int count = 0;
  StageResult result;
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    // TODO(SHU): need refactor
    if (count++ == frame->mutable_reference_line_info()->size()) {
      break;
    }
    ADEBUG << "No: [" << count << "] Reference Line.";
    ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath();
 
    if (has_drivable_reference_line) {
      reference_line_info.SetDrivable(false);
      break;
    }
 
    result =
        PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
 
    if (!result.HasError()) {
      if (!reference_line_info.IsChangeLanePath()) {
        ADEBUG << "reference line is NOT lane change ref.";
        has_drivable_reference_line = true;
        continue;
      }
      if (reference_line_info.Cost() < kStraightForwardLineCost) {
        // If the path and speed optimization succeed on target lane while
        // under smart lane-change or IsClearToChangeLane under older version
        has_drivable_reference_line = true;
        reference_line_info.SetDrivable(true);
      } else {
        reference_line_info.SetDrivable(false);
        ADEBUG << "\tlane change failed";
      }
    } else {
      reference_line_info.SetDrivable(false);
    }
  }
 
  return has_drivable_reference_line
             ? result.SetStageStatus(StageStatusType::RUNNING)
             : result.SetStageStatus(StageStatusType::ERROR);
}

RecordObstacleDebugInfo()

void apollo::planning::LaneFollowStage::RecordObstacleDebugInfo

(

ReferenceLineInfo *

reference_line_info

)

在文件 lane_follow_stage.cc 第 54 行定义.

                                            {
  if (!FLAGS_enable_record_debug) {
    ADEBUG << "Skip record debug info";
    return;
  }
  auto ptr_debug = reference_line_info->mutable_debug();
 
  const auto path_decision = reference_line_info->path_decision();
  for (const auto obstacle : path_decision->obstacles().Items()) {
    auto obstacle_debug = ptr_debug->mutable_planning_data()->add_obstacle();
    obstacle_debug->set_id(obstacle->Id());
    obstacle_debug->mutable_sl_boundary()->CopyFrom(
        obstacle->PerceptionSLBoundary());
    const auto& decider_tags = obstacle->decider_tags();
    const auto& decisions = obstacle->decisions();
    if (decider_tags.size() != decisions.size()) {
      AERROR << "decider_tags size: " << decider_tags.size()
             << " different from decisions size:" << decisions.size();
    }
    for (size_t i = 0; i < decider_tags.size(); ++i) {
      auto decision_tag = obstacle_debug->add_decision_tag();
      decision_tag->set_decider_tag(decider_tags[i]);
      decision_tag->mutable_decision()->CopyFrom(decisions[i]);
    }
  }
}

---

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

• modules/planning/scenarios/lane_follow/lane_follow_stage.h
• modules/planning/scenarios/lane_follow/lane_follow_stage.cc