Udacity 5.Behavior_Planning 5

udacity5_Behavior_Planner 设计5 vehicle.cpp 代码解析

vehicle.cpp 头文件代码解析

#include "vehicle.h"
#include <algorithm>
#include <iterator>
#include <map>
#include <string>
#include <vector>
#include "cost.h"

using std::string;
using std::vector;

// Initializes Vehicle
Vehicle::Vehicle(){}

Vehicle::Vehicle(int lane, float s, float v, float a, string state) {
  this->lane = lane;
  this->s = s;
  this->v = v;
  this->a = a;
  this->state = state;
  max_acceleration = -1;
}

Vehicle::~Vehicle() {}

vector<Vehicle> Vehicle::choose_next_state(map<int, vector<Vehicle>> &predictions) {
  /**
   * Here you can implement the transition_function code from the Behavior 
   *   Planning Pseudocode classroom concept.
   *
   * @param A predictions map. This is a map of vehicle id keys with predicted
   *   vehicle trajectories as values. Trajectories are a vector of Vehicle 
   *   objects representing the vehicle at the current timestep and one timestep
   *   in the future.//里面存放的是对其他车辆的预测信息
   * @output The best (lowest cost) trajectory corresponding to the next ego 
   *   vehicle state.下一个自我车辆最好的轨迹，从最低损失函数来考虑
   *
   * Functions that will be useful:
   * 1. successor_states - Uses the current state to return a vector of possible
   *    successor states for the finite state machine. 下一个状态，从状态机里得到
   * 2. generate_trajectory - Returns a vector of Vehicle objects representing 
   *    a vehicle trajectory, given a state and predictions. Note that 
   *    trajectory vectors might have size 0 if no possible trajectory exists 
   *    for the state. 生成轨迹
   * 3. calculate_cost - Included from cost.cpp, computes the cost for a trajectory.
   *    计算cost函数
   * TODO: Your solution here.
   */
  vector<string> states = successor_states();//获取接下来的状态
  float cost;
  vector<float> costs;
  vector<vector<Vehicle>> final_trajectories;

  for (vector<string>::iterator it = states.begin(); it != states.end(); ++it) {
    vector<Vehicle> trajectory = generate_trajectory(*it, predictions);//根据其他车辆的预测信息生成一个轨迹
    if (trajectory.size() != 0) {
      cost = calculate_cost(*this, predictions, trajectory);//计算cost
      costs.push_back(cost);
      final_trajectories.push_back(trajectory);//将可能状况都放入
    }
  }

  vector<float>::iterator best_cost = min_element(begin(costs), end(costs));
  int best_idx = distance(begin(costs), best_cost);求得哪一个idx的轨迹cost最小

  /**
   * TODO: Change return value here:
   */
  return final_trajectories[best_idx];//将最小的轨迹返回
}

vector<string> Vehicle::successor_states() {
  // Provides the possible next states given the current state for the FSM 
  //   discussed in the course, with the exception that lane changes happen 
  //   instantaneously, so LCL and LCR can only transition back to KL.
  vector<string> states;
  states.push_back("KL");
  string state = this->state;
  if(state.compare("KL") == 0) {
    states.push_back("PLCL");//优先考虑能否左转
    states.push_back("PLCR");
  } else if (state.compare("PLCL") == 0) {
    if (lane != lanes_available - 1) {
      states.push_back("PLCL");
      states.push_back("LCL");
    }
  } else if (state.compare("PLCR") == 0) {
    if (lane != 0) {
      states.push_back("PLCR");
      states.push_back("LCR");
    }
  }
    
  // If state is "LCL" or "LCR", then just return "KL"
  //如果现在状态是转弯，那下一个状态就是直行
  return states;
}

vector<Vehicle> Vehicle::generate_trajectory(string state, 
                                             map<int, vector<Vehicle>> &predictions) {
  // Given a possible next state, generate the appropriate trajectory to realize
  //   the next state.
  vector<Vehicle> trajectory;
  if (state.compare("CS") == 0) {
    trajectory = constant_speed_trajectory();//定速
  } else if (state.compare("KL") == 0) {
    trajectory = keep_lane_trajectory(predictions);//保持车道
  } else if (state.compare("LCL") == 0 || state.compare("LCR") == 0) {
    trajectory = lane_change_trajectory(state, predictions);//返回转弯的轨迹
  } else if (state.compare("PLCL") == 0 || state.compare("PLCR") == 0) {
    trajectory = prep_lane_change_trajectory(state, predictions);//返回预转弯的轨迹
  }

  return trajectory;
}

vector<float> Vehicle::get_kinematics(map<int, vector<Vehicle>> &predictions, 
                                      int lane) {//获取当前车辆下一刻的运动学
  // Gets next timestep kinematics (position, velocity, acceleration) 
  // 获取下一个时刻的运动学参数
  //   for a given lane. Tries to choose the maximum velocity and acceleration, 
  //   given other vehicle positions and accel/velocity constraints.
  float max_velocity_accel_limit = this->max_acceleration + this->v;//最大速度加速度限制
  float new_position;//新坐标
  float new_velocity;//新速度
  float new_accel;//新加速度
  Vehicle vehicle_ahead;//自我车辆的前车
  Vehicle vehicle_behind;//自我车辆的后车

  if (get_vehicle_ahead(predictions, lane, vehicle_ahead)) {//如果发现前面有车辆
    if (get_vehicle_behind(predictions, lane, vehicle_behind)) {//如果发现后面有车辆
      // must travel at the speed of traffic, regardless of preferred buffer
      new_velocity = vehicle_ahead.v;//则必须以交通速度行驶
    } else {//如果后面没发现车
        //前面车的距离，减去缓冲区，减去现在车的s，
        // v+0.5at^2 <=两车之间的距离减去缓冲区，加上前车在单位时间内行驶的距离
      float max_velocity_in_front = (vehicle_ahead.s - this->s 
                                  - this->preferred_buffer) + vehicle_ahead.v 
                                  - 0.5 * (this->a);
        //新的速度，肯定是三个最大速度里最小的一个
      new_velocity = std::min(std::min(max_velocity_in_front, 
                                       max_velocity_accel_limit), 
                                       this->target_speed);
    }
  } else {//如果前面没车，则需要加速，到限定速度
    new_velocity = std::min(max_velocity_accel_limit, this->target_speed);
  }
    //新加速度计算
  new_accel = new_velocity - this->v; // Equation: (v_1 - v_0)/t = acceleration
  //新地点计算
  new_position = this->s + new_velocity + new_accel / 2.0;
    //返回新的坐标，速度，加速度
  return{new_position, new_velocity, new_accel};
}

vector<Vehicle> Vehicle::constant_speed_trajectory() {//返回定常速度的轨迹
  // Generate a constant speed trajectory.
  float next_pos = position_at(1);
  //轨迹里放的是下一时刻车的所有信息，包含速度，车道，坐标，加速度
  vector<Vehicle> trajectory = {Vehicle(this->lane,this->s,this->v,this->a,this->state), 
                                Vehicle(this->lane,next_pos,this->v,0,this->state)};
  return trajectory;
}

vector<Vehicle> Vehicle::keep_lane_trajectory(map<int, vector<Vehicle>> &predictions) {
  // Generate a keep lane trajectory.保持直行的轨迹 保持直行的轨迹，但是不是定速的
  vector<Vehicle> trajectory = {Vehicle(lane, this->s, this->v, this->a, state)};
  vector<float> kinematics = get_kinematics(predictions, this->lane);
  float new_s = kinematics[0];
  float new_v = kinematics[1];
  float new_a = kinematics[2];
  trajectory.push_back(Vehicle(this->lane, new_s, new_v, new_a, "KL"));
  
  return trajectory;
}

vector<Vehicle> Vehicle::prep_lane_change_trajectory(string state, 
                                                     map<int, vector<Vehicle>> &predictions) {
  // Generate a trajectory preparing for a lane change.
  //准备转弯的轨迹
  float new_s;
  float new_v;
  float new_a;
  Vehicle vehicle_behind;//定义一个后面的车
  int new_lane = this->lane + lane_direction[state];//lane_direction[state]这是一个字典，向左是1向右是负一
  vector<Vehicle> trajectory = {Vehicle(this->lane, this->s, this->v, this->a, 
                                        this->state)};
    //在这条车道的下一刻的运动关系
  vector<float> curr_lane_new_kinematics = get_kinematics(predictions, this->lane);

  if (get_vehicle_behind(predictions, this->lane, vehicle_behind)) {
      //查看后面是不是有车，如果有车，则不进行动态变换
    // Keep speed of current lane so as not to collide with car behind.
    new_s = curr_lane_new_kinematics[0];
    new_v = curr_lane_new_kinematics[1];
    new_a = curr_lane_new_kinematics[2];    
  } else {//如果现在车辆后面没车
    vector<float> best_kinematics;
    //就进行新道路的状态量
    vector<float> next_lane_new_kinematics = get_kinematics(predictions, new_lane);
    // Choose kinematics with lowest velocity.
    //选择低速度的运动学状态
    if (next_lane_new_kinematics[1] < curr_lane_new_kinematics[1]) {
      best_kinematics = next_lane_new_kinematics;
    } else {
      best_kinematics = curr_lane_new_kinematics;
    }
    new_s = best_kinematics[0];
    new_v = best_kinematics[1];
    new_a = best_kinematics[2];
  }

  trajectory.push_back(Vehicle(this->lane, new_s, new_v, new_a, state));
  
  return trajectory;
}
//————————————减速换道 所以需要判断后方有没有车，但是应该判断的是侧后方啊？代码理解错了？
vector<Vehicle> Vehicle::lane_change_trajectory(string state, //换道轨迹
                                                map<int, vector<Vehicle>> &predictions) {
  // Generate a lane change trajectory.
  int new_lane = this->lane + lane_direction[state];
  vector<Vehicle> trajectory;
  Vehicle next_lane_vehicle;
  // Check if a lane change is possible (check if another vehicle occupies 
  //   that spot).检测是否可以换道，检查是否有其他车辆占用那个点
  for (map<int, vector<Vehicle>>::iterator it = predictions.begin(); 
       it != predictions.end(); ++it) {//迭代对其他车辆的预测信息
    next_lane_vehicle = it->second[0];//提取车辆
    if (next_lane_vehicle.s == this->s && next_lane_vehicle.lane == new_lane) {
      // If lane change is not possible, return empty trajectory.
      return trajectory;//占据了新车道下的位置，代表不可以变道，直接返回空
    }
  }
  //遍历完后还不返回空，代表可以变道
  trajectory.push_back(Vehicle(this->lane, this->s, this->v, this->a, 
                               this->state));
  vector<float> kinematics = get_kinematics(predictions, new_lane);
  trajectory.push_back(Vehicle(new_lane, kinematics[0], kinematics[1], 
                               kinematics[2], state));
  return trajectory;//返回两个新的轨迹点，代表是可以变道的
}

void Vehicle::increment(int dt = 1) {
  this->s = position_at(dt);
}

float Vehicle::position_at(int t) {//找出下一刻的坐标
  return this->s + this->v*t + this->a*t*t/2.0;
}

bool Vehicle::get_vehicle_behind(map<int, vector<Vehicle>> &predictions, 
                                 int lane, Vehicle &rVehicle) {//查找后方是否有车
  // Returns a true if a vehicle is found behind the current vehicle, false 
  //   otherwise. The passed reference rVehicle is updated if a vehicle is found.
  int max_s = -1;
  bool found_vehicle = false;
  Vehicle temp_vehicle;
  for (map<int, vector<Vehicle>>::iterator it = predictions.begin(); 
       it != predictions.end(); ++it) {
    temp_vehicle = it->second[0];
    if (temp_vehicle.lane == this->lane && temp_vehicle.s < this->s 
        && temp_vehicle.s > max_s) {
      max_s = temp_vehicle.s;
      rVehicle = temp_vehicle;
      found_vehicle = true;
    }
  }
  
  return found_vehicle;
}
// 获取前车的信息，返回前面是否发现车辆
bool Vehicle::get_vehicle_ahead(map<int, vector<Vehicle>> &predictions, 
                                int lane, Vehicle &rVehicle) {
  // Returns a true if a vehicle is found ahead of the current vehicle, false 
  //   otherwise. The passed reference rVehicle is updated if a vehicle is found.
  int min_s = this->goal_s;
  bool found_vehicle = false;
  Vehicle temp_vehicle;
  for (map<int, vector<Vehicle>>::iterator it = predictions.begin(); 
       it != predictions.end(); ++it) {//对每个车辆的预测信息开始循环
    temp_vehicle = it->second[0];
    if (temp_vehicle.lane == this->lane && temp_vehicle.s > this->s 
        && temp_vehicle.s < min_s) {
      min_s = temp_vehicle.s;
      rVehicle = temp_vehicle;
      found_vehicle = true;
    }
  }
  
  return found_vehicle;
}
//生成车辆的预测信息，这个使用是在road里使用的
vector<Vehicle> Vehicle::generate_predictions(int horizon) {
  // Generates predictions for non-ego vehicles to be used in trajectory 
  //   generation for the ego vehicle.
  vector<Vehicle> predictions;
  for(int i = 0; i < horizon; ++i) {
    float next_s = position_at(i);//当前车辆下一时间的位置
    float next_v = 0;//当前车辆下一时间的速度
    if (i < horizon-1) {
      next_v = position_at(i+1) - s;
    }
    predictions.push_back(Vehicle(this->lane, next_s, next_v, 0));
  }
  
  return predictions;
}
//实现下一个状态
void Vehicle::realize_next_state(vector<Vehicle> &trajectory) {
  // Sets state and kinematics for ego vehicle using the last state of the trajectory.
  Vehicle next_state = trajectory[1];
  this->state = next_state.state;
  this->lane = next_state.lane;
  this->s = next_state.s;
  this->v = next_state.v;
  this->a = next_state.a;
}
//汽车配置
void Vehicle::configure(vector<int> &road_data) {
  // Called by simulator before simulation begins. Sets various parameters which
  //   will impact the ego vehicle.
  target_speed = road_data[0];
  lanes_available = road_data[1];
  goal_s = road_data[2];
  goal_lane = road_data[3];
  max_acceleration = road_data[4];
}