src/main.cpp
//! Method which generates the trajectory given car position and target lane
//! @param - car_s - S position of car
//! @param - f_lane - Target lane
//! @param - map_waypoints_s, map_waypoints_x, map_waypoints_y - For conversion from Frenet to XY coordinates
//! @param - spline_xs, spline_ys - Trailing (older) points in the spline for smoother transition
//! @param - previous_path_x, previous_path_y - List of previous X and previous Y points to be prefixed to the trajectory
//! @param - f_traj - [out] - The output trajectory
Method which generates the trajectory given car position and target lane
Method which generates the trajectory given car position and target lane
void generateTrajectory(double car_s, int f_lane, 
                        vector<double>& map_waypoints_s, vector<double>& map_waypoints_x, vector<double>& map_waypoints_y, 
                        vector<double> spline_xs, vector<double> spline_ys, 
                        vector<double>& previous_path_x, vector<double>& previous_path_y, 
                        STrajectory& f_traj)
{
  vector<double> wp1 = getXY(car_s + horizon_front, (f_lane*4)+2, map_waypoints_s, map_waypoints_x, map_waypoints_y);
  vector<double> wp2 = getXY(car_s + horizon_front*1.5, (f_lane*4)+2, map_waypoints_s, map_waypoints_x, map_waypoints_y);
  vector<double> wp3 = getXY(car_s + horizon_front*2, (f_lane*4)+2, map_waypoints_s, map_waypoints_x, map_waypoints_y);
  spline_xs.push_back(wp1[0]);
  spline_xs.push_back(wp2[0]);
  spline_xs.push_back(wp3[0]);
  spline_ys.push_back(wp1[1]);
  spline_ys.push_back(wp2[1]);
  spline_ys.push_back(wp3[1]);
  double ref_x = spline_xs[1];
  double ref_y = spline_ys[1];
  double ref_yaw = atan2(ref_y - spline_ys[0], ref_x - spline_xs[0]);
  for(int count = 0; count < spline_xs.size(); count++)
  {
    double trans_x = spline_xs[count] - ref_x;
    double trans_y = spline_ys[count] - ref_y;
    spline_xs[count] = trans_x * cos(-ref_yaw) - trans_y * sin(-ref_yaw);
    spline_ys[count] = trans_x * sin(-ref_yaw) + trans_y * cos(-ref_yaw);
  }
  for(int count = 0; count < previous_path_x.size(); count++)
  {
    f_traj.next_x_vals.push_back(previous_path_x[count]);
    f_traj.next_y_vals.push_back(previous_path_y[count]);
  }
  ::tk::spline s;
  s.set_points(spline_xs, spline_ys);
  double x = horizon_front;
  double y = s(x);
  double dist = distance(x, y, 0, 0);
  double N = dist/(0.02 * cur_vel / 2.237);
  double x_step = x/N;
  for(int count = 1; count <= 50 - previous_path_x.size(); count++)
  {
    double x_val = x_step*count;
    double y_val = s(x_val);
    double x_rotated = x_val * cos(ref_yaw) - y_val * sin(ref_yaw);
    double y_rotated = x_val * sin(ref_yaw) + y_val * cos(ref_yaw);
    f_traj.next_x_vals.push_back(x_rotated + ref_x);
    f_traj.next_y_vals.push_back(y_rotated + ref_y);
  }
}
void generateTrajectory ( double car_s , int f_lane , vector < double > & map_waypoints_s , vector < double > & map_waypoints_x , vector < double > & map_waypoints_y , vector < double > spline_xs , vector < double > spline_ys , vector < double > & previous_path_x , vector < double > & previous_path_y , STrajectory & f_traj ) { vector < double > wp1 = getXY ( car_s + horizon_front , ( f_lane * 4 ) + 2 , map_waypoints_s , map_waypoints_x , map_waypoints_y ) ; vector < double > wp2 = getXY ( car_s + horizon_front * 1.5 , ( f_lane * 4 ) + 2 , map_waypoints_s , map_waypoints_x , map_waypoints_y ) ; vector < double > wp3 = getXY ( car_s + horizon_front * 2 , ( f_lane * 4 ) + 2 , map_waypoints_s , map_waypoints_x , map_waypoints_y ) ; spline_xs . push_back ( wp1 [ 0 ] ) ; spline_xs . push_back ( wp2 [ 0 ] ) ; spline_xs . push_back ( wp3 [ 0 ] ) ; spline_ys . push_back ( wp1 [ 1 ] ) ; spline_ys . push_back ( wp2 [ 1 ] ) ; spline_ys . push_back ( wp3 [ 1 ] ) ; double ref_x = spline_xs [ 1 ] ; double ref_y = spline_ys [ 1 ] ; double ref_yaw = atan2 ( ref_y - spline_ys [ 0 ] , ref_x - spline_xs [ 0 ] ) ; for ( int count = 0 ; count < spline_xs . size ( ) ; count ++ ) { double trans_x = spline_xs [ count ] - ref_x ; double trans_y = spline_ys [ count ] - ref_y ; spline_xs [ count ] = trans_x * cos ( - ref_yaw ) - trans_y * sin ( - ref_yaw ) ; spline_ys [ count ] = trans_x * sin ( - ref_yaw ) + trans_y * cos ( - ref_yaw ) ; } for ( int count = 0 ; count < previous_path_x . size ( ) ; count ++ ) { f_traj . next_x_vals . push_back ( previous_path_x [ count ] ) ; f_traj . next_y_vals . push_back ( previous_path_y [ count ] ) ; } :: tk :: spline s ; s . set_points ( spline_xs , spline_ys ) ; double x = horizon_front ; double y = s ( x ) ; double dist = distance ( x , y , 0 , 0 ) ; double N = dist / ( 0.02 * cur_vel / 2.237 ) ; double x_step = x / N ; for ( int count = 1 ; count <= 50 - previous_path_x . size ( ) ; count ++ ) { double x_val = x_step * count ; double y_val = s ( x_val ) ; double x_rotated = x_val * cos ( ref_yaw ) - y_val * sin ( ref_yaw ) ; double y_rotated = x_val * sin ( ref_yaw ) + y_val * cos ( ref_yaw ) ; f_traj . next_x_vals . push_back ( x_rotated + ref_x ) ; f_traj . next_y_vals . push_back ( y_rotated + ref_y ) ; } }
Method which generates the trajectory given car position and target lane
Method which generates the trajectory given car position and target lane
//! Horizon of 3*horizon_front metres //! Do the translation and rotation to transform the origin //! store the previous paths //printf("set_spline\n"); //2.237 is the factor to convert mph to m/s //! Calculate the remaining points and append to trajectory