getNextStates()函数的功能

 

/**
 * Compute the possible next moves according to
 * the kinematic bicycle model constraints.
 *根据运动自行车模型约束计算可能的下一步移动。
 * Kinematic Bicycle Model centered the rear wheel.运动自行车模型以后轮为中心。
 *
 */
 //vector<State> 返回值类型   State::对象内部函数
std::vector<State> State::getNextStates()
{
  std::vector<State> next;
  State state;

  // Bicycle model parameters自行车模型参数
  double alpha = 0;
  double d_theta = 0;
  double radius = 0; // rotation radius转弯半径
  double dist = 30.0; // distance travelled within a unit time
                      //单位时间内的行进距离
  // Try out possible values of alpha尝试可能的alpha值
  // among 3 angles: [-VEH_M_ALPHA, 0, VEH_M_ALPHA].
  for (alpha = -VEH_M_ALPHA; alpha <= VEH_M_ALPHA + 0.001;alpha += VEH_M_ALPHA)
  {
    //单位时间航向角的增量
    d_theta = dist * tan(alpha * PI / 180) / VEH_LEN;//#define VEH_LEN 34   车辆轴距

    if (abs(d_theta) < 0.001) { // Forward 前进
      state.x = x + dist * cos(theta * 2.0 * PI / Theta);//#define Theta 72
      //theta * 2.0 * PI / Theta  不懂这样算的目的是什么
      state.y = y + dist * sin(theta * 2.0 * PI / Theta);
      state.theta = theta;
    }
    else
    { // Turning 转向
      // Bicycle model centered at the rear wheel以后轮为中心的自行车模型
      radius = VEH_LEN / tan(alpha * PI / 180);

        //#define Theta 72 // maximal angle
        //#define Theta_Res 5 // angle resolution 角度分辨率
      state.x = x + radius * sin(theta * 2.0 * PI / Theta + d_theta)
                  - radius * sin(theta * 2.0 * PI / Theta);
      state.y = y - radius * cos(theta * 2.0 * PI / Theta + d_theta)
                  + radius * cos(theta * 2.0 * PI / Theta);
        

      // new theta = theta + d_theta
      double new_theta = theta + d_theta * 180 / PI / Theta_Res;

      // normalize
      if (new_theta > 0) {
        state.theta = fmod(new_theta, Theta);//new_theta除以Theta之后的余数
      }
      else {
        state.theta = new_theta + Theta;
      }
    }

    // Convert to grid-map coordinates.
    state.gx = state.x / Grid_Res;//#define Grid_Res 10
    state.gy = state.y / Grid_Res;
    state.gtheta = state.theta + 0.01;

    next.push_back(state);
  }


  return next;
}