apollo自动驾驶进阶学习之：如何实现左转待转思路解析

首先需要理解Planning模块是基于Scenario、Stage、Task这样的层次来进行的，即：场景->步骤->具体的决策方法。Apollo可以应对自动驾驶所面临的不同道路场景，都是通过Scenario统一注册与管理。Scenario通过一个有限状态机来判断选择当前行车场景，每个Scenario下又有多个Stage，指当前场景下需要执行的粗略步骤。

Planning模块根据routing（导航模块），prediction（感知模块）感知的周围环境信息，以及地图定位导航信息为自动驾驶车辆规划出一条运动轨迹（包含坐标，速度，加速度，jerk加加速度，时间等信息），然后将这些信息传递给控制模块。

1、stage_approach

代码路径：modules/planning/scenarios/traffic_light/unprotected_left_turn/stage_approach.cc
本部分实现的功能实现直行红绿灯的判断，等待直行绿灯后进入待转区。

/**
 * @file
 **/

#include "modules/planning/scenarios/traffic_light/unprotected_left_turn/stage_approach.h"
#include <iterator>
#include "cyber/common/log.h"
#include "cyber/time/clock.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/perception/proto/traffic_light_detection.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/speed_profile_generator.h"
#include "modules/planning/scenarios/util/util.h"
#include "modules/planning/tasks/deciders/creep_decider/creep_decider.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace traffic_light {

using apollo::common::TrajectoryPoint;
using apollo::cyber::Clock;
using apollo::hdmap::PathOverlap;
using apollo::perception::TrafficLight;

/*sam_start**********************/
using apollo::hdmap::HDMapUtil;
/*sam_end***********************/

Stage::StageStatus TrafficLightUnprotectedLeftTurnStageApproach::Process(
    const TrajectoryPoint& planning_init_point, Frame* frame) {
  ADEBUG << "stage: Approach";
  CHECK_NOTNULL(frame);

  scenario_config_.CopyFrom(GetContext()->scenario_config);

  if (!config_.enabled()) {
    return FinishStage(frame);
  }

  // set cruise_speed to slow down
  frame->mutable_reference_line_info()->front().SetCruiseSpeed(
      scenario_config_.approach_cruise_speed());

  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
  if (!plan_ok) {
    AERROR << "TrafficLightUnprotectedLeftTurnStageApproach planning error";
  }

  if (GetContext()->current_traffic_light_overlap_ids.empty()) {
    return FinishScenario();
  }

  const auto& reference_line_info = frame->reference_line_info().front();

  const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  /*sam_start**********************/
  const auto hdmap_ptr = HDMapUtil::BaseMapPtr();
  /*sam_end***********************/

  PathOverlap* traffic_light = nullptr;
  bool traffic_light_all_done = true;
  for (const auto& traffic_light_overlap_id :
       GetContext()->current_traffic_light_overlap_ids) {
    // get overlap along reference line
    PathOverlap* current_traffic_light_overlap =
        scenario::util::GetOverlapOnReferenceLine(reference_line_info,
                                                  traffic_light_overlap_id,
                                                  ReferenceLineInfo::SIGNAL);
    if (!current_traffic_light_overlap) {
      continue;
    }
    
    /*ss_start**********************/
    //获取左转红绿灯的状态
    apollo::hdmap::Id signal_id;
    signal_id.set_id(current_traffic_light_overlap->object_id);
    auto signal = hdmap_ptr->GetSignalById(signal_id)->signal();
    TrafficLight::Color signal_color = TrafficLight::UNKNOWN;
    for (auto subsignal : signal.subsignal()) {
      if (subsignal.type() == apollo::hdmap::Subsignal::CIRCLE) {
        signal_color = frame->GetSignal(subsignal.id().id()).color();
        break;
      }
    }
    /*ss_end***********************/

    // set right_of_way_status
    reference_line_info.SetJunctionRightOfWay(
        current_traffic_light_overlap->start_s, false);




    const double distance_adc_to_stop_line =
        std::min(signal.stop_line().cbegin()->segment().cbegin()->s(),signal.stop_line().cend()->segment().begin()->s()) - adc_front_edge_s;
    /*ss_comment_start**********************/    
    //auto signal_color = frame->GetSignal(traffic_light_overlap_id).color();
    /*ss_comment_end***********************/
    ADEBUG << "traffic_light_overlap_id[" << traffic_light_overlap_id
           << "] start_s[" << current_traffic_light_overlap->start_s
           << "] distance_adc_to_stop_line[" << distance_adc_to_stop_line
           << "] color[" << signal_color << "]";

    if (distance_adc_to_stop_line < 0)
        return FinishStage(frame);
    // check on traffic light color and distance to stop line
    if (signal_color != TrafficLight::GREEN ||
        distance_adc_to_stop_line >=
            scenario_config_.max_valid_stop_distance()) {
      traffic_light_all_done = false;
      break;
    }
  }

  if (traffic_light == nullptr) {
    return FinishScenario();
  }

  if (traffic_light_all_done) {
    return FinishStage(frame);
  }

  return Stage::RUNNING;
}

Stage::StageStatus TrafficLightUnprotectedLeftTurnStageApproach::FinishStage(
    Frame* frame) {
  // check speed at stop_stage
  const double adc_speed = injector_->vehicle_state()->linear_velocity();
  if (adc_speed > scenario_config_.max_adc_speed_before_creep()) {
    // skip creep
    next_stage_ = ScenarioConfig ::
        TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN_INTERSECTION_CRUISE;
  } else {
    // creep
    // update PlanningContext
    injector_->planning_context()
        ->mutable_planning_status()
        ->mutable_traffic_light()
        ->mutable_done_traffic_light_overlap_id()
        ->Clear();
    for (const auto& traffic_light_overlap_id :
         GetContext()->current_traffic_light_overlap_ids) {
      injector_->planning_context()
          ->mutable_planning_status()
          ->mutable_traffic_light()
          ->add_done_traffic_light_overlap_id(traffic_light_overlap_id);
    }

    GetContext()->creep_start_time = Clock::NowInSeconds();
    next_stage_ = ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN_CREEP;
  }

  // reset cruise_speed
  auto& reference_line_info = frame->mutable_reference_line_info()->front();
  reference_line_info.SetCruiseSpeed(FLAGS_default_cruise_speed);

  return Stage::FINISHED;
}

}  // namespace traffic_light
}  // namespace scenario
}  // namespace planning
}  // namespace apollo

2、stage_creep

代码路径：modules/planning/scenarios/traffic_light/unprotected_left_turn/stage_creep.cc
本部代码需要复制上面stage_approach代码，需要将圆灯改成左转灯，等待左转灯绿灯后进入下一个stage。