apollo自动驾驶进阶学习之：TrafficDecider-＞TrafficLight

    adc_distance_to_traffic_light <
          scenario_config.start_traffic_light_scenario_distance()

    [
    {
        "type":"3D Ground Truth",
        "name":"3D Ground Truth",
        "params":{
            "Frequency":10,
            "MaxDistance":100,
            "Topic":"/apollo/perception/obstacles"
        },
        "transform":{
            "x":0,
            "y":0,
            "z":0,
            "pitch":0,
            "yaw":0,
            "roll":0
        }
    },
    {
        "type":"Signal",
        "name":"Signal Sensor",
        "params":{
            "Frequency":10,
            "MaxDistance":100,
            "Topic":"/apollo/perception/traffic_light"
        },
        "transform":{
            "x":0,
            "y":0,
            "z":0,
            "pitch":0,
            "yaw":0,
            "roll":0
        }
    },
    {
        "type":"CAN-Bus",
        "name":"CAN Bus",
        "params":{
            "Frequency":10,
            "Topic":"/apollo/canbus/chassis"
        },
        "transform":{
            "x":0,
            "y":0,
            "z":0,
            "pitch":0,
            "yaw":0,
            "roll":0
        }
    },
    {
        "type":"GPS Device",
        "name":"GPS",
        "params":{
            "Frequency":12.5,
            "Topic":"/apollo/sensor/gnss/best_pose",
            "Frame":"gps"
        },
        "transform":{
            "x":0,
            "y":0,
            "z":-1.348649,
            "pitch":0,
            "yaw":0,
            "roll":0
        }
    },
    {
        "type":"GPS Odometry",
        "name":"GPS Odometry",
        "params":{
            "Frequency":12.5,
            "Topic":"/apollo/sensor/gnss/odometry",
            "Frame":"gps"
        },
        "transform":{
            "x":0,
            "y":0,
            "z":-1.348649,
            "pitch":0,
            "yaw":0,
            "roll":0
        }
    },
    {
        "type":"GPS-INS Status",
        "name":"GPS INS Status",
        "params":{
            "Frequency":12.5,
            "Topic":"/apollo/sensor/gnss/ins_stat",
            "Frame":"gps"
        },
        "transform":{
            "x":0,
            "y":0,
            "z":-1.348649,
            "pitch":0,
            "yaw":0,
            "roll":0
        }
    },
    {
        "type":"IMU",
        "name":"IMU",
        "params":{
            "Topic":"/apollo/sensor/gnss/imu",
            "Frame":"imu",
            "CorrectedTopic":"/apollo/sensor/gnss/corrected_imu",
            "CorrectedFrame":"imu"
        },
        "transform":{
            "x":0,
            "y":0,
            "z":-1.348649,
            "pitch":0,
            "yaw":0,
            "roll":0
        }
    },
    {
        "type":"Radar",
        "name":"Radar",
        "params":{
            "Frequency":13.4,
            "Topic":"/apollo/sensor/conti_radar"
        },
        "transform":{
            "x":0,
            "y":0.689,
            "z":2.272,
            "pitch":0,
            "yaw":0,
            "roll":0
        }
    },
    {
        "type":"Keyboard Control",
        "name":"Keyboard Car Control"
    },
    {
        "type":"Wheel Control",
        "name":"Wheel Car Control"
    },
    {
        "type":"Vehicle Control",
        "name":"Apollo Car Control",
        "params":{
            "Topic":"/apollo/control"
        }
    }
    ]

    scenario_type: TRAFFIC_LIGHT_PROTECTED
    traffic_light_protected_config: {
      start_traffic_light_scenario_distance: 5.0
      max_valid_stop_distance: 2.0
      min_pass_s_distance: 3.0
    }
    stage_type: TRAFFIC_LIGHT_PROTECTED_APPROACH
    stage_type: TRAFFIC_LIGHT_PROTECTED_INTERSECTION_CRUISE
    
    stage_config: {
      stage_type: TRAFFIC_LIGHT_PROTECTED_APPROACH
      enabled: true
      task_type: PATH_LANE_BORROW_DECIDER
      task_type: PATH_BOUNDS_DECIDER
      task_type: PIECEWISE_JERK_PATH_OPTIMIZER
      task_type: PATH_ASSESSMENT_DECIDER
      task_type: PATH_DECIDER
      task_type: RULE_BASED_STOP_DECIDER
      task_type: ST_BOUNDS_DECIDER
      task_type: SPEED_BOUNDS_PRIORI_DECIDER
      task_type: SPEED_HEURISTIC_OPTIMIZER
      task_type: SPEED_DECIDER
      task_type: SPEED_BOUNDS_FINAL_DECIDER
      task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
    
    
    stage_config: {
      stage_type: TRAFFIC_LIGHT_PROTECTED_INTERSECTION_CRUISE
      enabled: true
      task_type: PATH_LANE_BORROW_DECIDER
      task_type: PATH_BOUNDS_DECIDER
      task_type: PIECEWISE_JERK_PATH_OPTIMIZER
      task_type: PATH_ASSESSMENT_DECIDER
      task_type: PATH_DECIDER
      task_type: RULE_BASED_STOP_DECIDER
      task_type: ST_BOUNDS_DECIDER
      task_type: SPEED_BOUNDS_PRIORI_DECIDER
      task_type: SPEED_HEURISTIC_OPTIMIZER
      task_type: SPEED_DECIDER
      task_type: SPEED_BOUNDS_FINAL_DECIDER
      task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
      task_config: {
    task_type: PATH_LANE_BORROW_DECIDER
    path_lane_borrow_decider_config {
      allow_lane_borrowing: true
    }
      }

      for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {
    TrafficDecider traffic_decider;
    traffic_decider.Init(traffic_rule_configs_);
    auto traffic_status =
        traffic_decider.Execute(frame_.get(), &ref_line_info, injector_);
    if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {
      ref_line_info.SetDrivable(false);
      AWARN << "Reference line " << ref_line_info.Lanes().Id()
            << " traffic decider failed";
    }
      }

    void TrafficLight::MakeDecisions(Frame* const frame,
                                 ReferenceLineInfo* const reference_line_info) {
      CHECK_NOTNULL(frame);
      CHECK_NOTNULL(reference_line_info);
      //查看配置文件中交通灯规则是否开启
      if (!config_.traffic_light().enabled()) {
    return;
      }
      //查看交通灯信号状态
      const auto& traffic_light_status =
      injector_->planning_context()->planning_status().traffic_light();
      //车前位置
      const double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s();
      //车位位置
      const double adc_back_edge_s = reference_line_info->AdcSlBoundary().start_s();
    
      // debug info调试信息
      planning_internal::SignalLightDebug* signal_light_debug =
      reference_line_info->mutable_debug()
          ->mutable_planning_data()
          ->mutable_signal_light();
      signal_light_debug->set_adc_front_s(adc_front_edge_s);
      signal_light_debug->set_adc_speed(
      injector_->vehicle_state()->linear_velocity());
    
      const std::vector<PathOverlap>& traffic_light_overlaps =
      reference_line_info->reference_line().map_path().signal_overlaps();
      //遍历所有与规划轨迹重叠的交通信号灯
      for (const auto& traffic_light_overlap : traffic_light_overlaps) {
    //如果已经超过的红绿灯，忽略
    if (traffic_light_overlap.end_s <= adc_back_edge_s) {
      continue;
    }
    
    // check if traffic-light-stop already finished, set by scenario/stage
    //查看是否已经是已经通过停止完成了，通过场景调用部分，如果已经停止过了，则忽略，继续下个循环
    bool traffic_light_done = false;
    for (const auto& done_traffic_light_overlap_id :
         traffic_light_status.done_traffic_light_overlap_id()) {
      if (traffic_light_overlap.object_id == done_traffic_light_overlap_id) {
        traffic_light_done = true;
        break;
        //跳出循环
      }
    }
    if (traffic_light_done) {
      continue;
      //继续下个循环
    }
    
    // work around incorrect s-projection along round routing
    static constexpr double kSDiscrepanceTolerance = 10.0;
    const auto& reference_line = reference_line_info->reference_line();
    common::SLPoint traffic_light_sl;
    //获取红路灯sl坐标
    traffic_light_sl.set_s(traffic_light_overlap.start_s);
    traffic_light_sl.set_l(0);
    common::math::Vec2d traffic_light_point;
    reference_line.SLToXY(traffic_light_sl, &traffic_light_point);
    //将红绿灯坐标转换到xy坐标
    common::math::Vec2d adc_position = {injector_->vehicle_state()->x(),
                                        injector_->vehicle_state()->y()};
    //计算车辆到红绿灯的xy坐标下的距离
    const double distance =
        common::util::DistanceXY(traffic_light_point, adc_position);
    //计算ls坐标下的距离
    const double s_distance = traffic_light_overlap.start_s - adc_front_edge_s;
    ADEBUG << "traffic_light[" << traffic_light_overlap.object_id
           << "] start_s[" << traffic_light_overlap.start_s << "] s_distance["
           << s_distance << "] actual_distance[" << distance << "]";
    
    //跳过不在参考线上的红绿灯,忽略不考虑，比如上图这种情况
    if (s_distance >= 0 &&
        fabs(s_distance - distance) > kSDiscrepanceTolerance) {
      ADEBUG << "SKIP traffic_light[" << traffic_light_overlap.object_id
             << "] close in position, but far away along reference line";
      continue;
    }
    
    
       //获取信号灯颜色
    auto signal_color =
        frame->GetSignal(traffic_light_overlap.object_id).color();
    //计算如果停止的话，计算停车加速度
    const double stop_deceleration = util::GetADCStopDeceleration(
        injector_->vehicle_state(), adc_front_edge_s,
        traffic_light_overlap.start_s);
    ADEBUG << "traffic_light_id[" << traffic_light_overlap.object_id
           << "] start_s[" << traffic_light_overlap.start_s << "] color["
           << signal_color << "] stop_deceleration[" << stop_deceleration
           << "]";
    
    // debug info调试信息
    planning_internal::SignalLightDebug::SignalDebug* signal_debug =
        signal_light_debug->add_signal();
    signal_debug->set_adc_stop_deceleration(stop_deceleration);
    signal_debug->set_color(signal_color);
    signal_debug->set_light_id(traffic_light_overlap.object_id);
    signal_debug->set_light_stop_s(traffic_light_overlap.start_s);
    
    //如果是绿灯，跳过，继续下个循环
    if (signal_color == perception::TrafficLight::GREEN) {
      continue;
    }
    
    // Red/Yellow/Unknown: check deceleration
    //如果停车超过了车辆最大减速度，则忽略，继续下个循环
    if (stop_deceleration > config_.traffic_light().max_stop_deceleration()) {
      AWARN << "stop_deceleration too big to achieve.  SKIP red light";
      continue;
    }
    
    // build stop decision
    //创建停车决策，创建虚拟障碍物
    ADEBUG << "BuildStopDecision: traffic_light["
           << traffic_light_overlap.object_id << "] start_s["
           << traffic_light_overlap.start_s << "]";
    std::string virtual_obstacle_id =
        TRAFFIC_LIGHT_VO_ID_PREFIX + traffic_light_overlap.object_id;
    const std::vector<std::string> wait_for_obstacles;
    util::BuildStopDecision(virtual_obstacle_id, traffic_light_overlap.start_s,
                            config_.traffic_light().stop_distance(),
                            StopReasonCode::STOP_REASON_SIGNAL,
                            wait_for_obstacles,
                            TrafficRuleConfig::RuleId_Name(config_.rule_id()),
                            frame, reference_line_info);
      }
    }

    int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s,
                      const double stop_distance,
                      const StopReasonCode& stop_reason_code,
                      const std::vector<std::string>& wait_for_obstacles,
                      const std::string& decision_tag, Frame* const frame,
                      ReferenceLineInfo* const reference_line_info) {
      CHECK_NOTNULL(frame);
      CHECK_NOTNULL(reference_line_info);
    
      // check
      
      const auto& reference_line = reference_line_info->reference_line();
      //判断停车点stop_line_s是否在[0 reference_line.Length()]范围内
      if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) {
    AERROR << "stop_line_s[" << stop_line_s << "] is not on reference line";
    return 0;
      }
      
      // create virtual stop wall创建虚拟墙
      //将障碍信息更新到frame中的obstacles_成员变量中
      const auto* obstacle =
      frame->CreateStopObstacle(reference_line_info, stop_wall_id, stop_line_s);
      if (!obstacle) {
    AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
    return -1;
      }
      //将障碍墙信息更新到reference_line->path_decision_中，更新障碍物s,t信息
      const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
      if (!stop_wall) {
    AERROR << "Failed to add obstacle[" << stop_wall_id << "]";
    return -1;
      }
    
      // build stop decision
      //获取停车s值
      const double stop_s = stop_line_s - stop_distance;
      //通过停车点s值计算出停车坐标点
      const auto& stop_point = reference_line.GetReferencePoint(stop_s);
      //获取朝向角
      const double stop_heading =
      reference_line.GetReferencePoint(stop_s).heading();
      //设置stop信息，比如停止原因，停车距离，停车时的朝向角，停车点的坐标
      ObjectDecisionType stop;
      auto* stop_decision = stop.mutable_stop();
      stop_decision->set_reason_code(stop_reason_code);
      stop_decision->set_distance_s(-stop_distance);
      stop_decision->set_stop_heading(stop_heading);
      stop_decision->mutable_stop_point()->set_x(stop_point.x());
      stop_decision->mutable_stop_point()->set_y(stop_point.y());
      stop_decision->mutable_stop_point()->set_z(0.0);
    
      for (size_t i = 0; i < wait_for_obstacles.size(); ++i) {
    stop_decision->add_wait_for_obstacle(wait_for_obstacles[i]);
      }
      //将停止信息更新到path_decision_中
      auto* path_decision = reference_line_info->path_decision();
      path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);
    
      return 0;
    }

    Status STBoundaryMapper::ComputeSTBoundary(PathDecision* path_decision) const {
      // Sanity checks.
      CHECK_GT(planning_max_time_, 0.0);
      if (path_data_.discretized_path().size() < 2) {
    AERROR << "Fail to get params because of too few path points. path points "
              "size: "
           << path_data_.discretized_path().size() << ".";
    return Status(ErrorCode::PLANNING_ERROR,
                  "Fail to get params because of too few path points");
      }
    
      // Go through every obstacle.
      Obstacle* stop_obstacle = nullptr;
      ObjectDecisionType stop_decision;
      double min_stop_s = std::numeric_limits<double>::max();
      for (const auto* ptr_obstacle_item : path_decision->obstacles().Items()) {
    Obstacle* ptr_obstacle = path_decision->Find(ptr_obstacle_item->Id());
    ACHECK(ptr_obstacle != nullptr);
    
    // If no longitudinal decision has been made, then plot it onto ST-graph.
    if (!ptr_obstacle->HasLongitudinalDecision()) {
      ComputeSTBoundary(ptr_obstacle);
      continue;
    }
    
    // If there is a longitudinal decision, then fine-tune boundary.
    const auto& decision = ptr_obstacle->LongitudinalDecision();
    if (decision.has_stop()) {
      // 1. Store the closest stop fence info.
      // TODO(all): store ref. s value in stop decision; refine the code then.
      common::SLPoint stop_sl_point;
      reference_line_.XYToSL(decision.stop().stop_point(), &stop_sl_point);
      const double stop_s = stop_sl_point.s();
    
      if (stop_s < min_stop_s) {
        stop_obstacle = ptr_obstacle;
        min_stop_s = stop_s;
        stop_decision = decision;
      }
    } else if (decision.has_follow() || decision.has_overtake() ||
               decision.has_yield()) {
      // 2. Depending on the longitudinal overtake/yield decision,
      //    fine-tune the upper/lower st-boundary of related obstacles.
      ComputeSTBoundaryWithDecision(ptr_obstacle, decision);
    } else if (!decision.has_ignore()) {
      // 3. Ignore those unrelated obstacles.
      AWARN << "No mapping for decision: " << decision.DebugString();
    }
      }
      if (stop_obstacle) {
    bool success = MapStopDecision(stop_obstacle, stop_decision);
    if (!success) {
      const std::string msg = "Fail to MapStopDecision.";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
      }
    
      return Status::OK();
    }