src/main.cpp
//! Method which detects whether a collision is possible at N timestamps ahead
//! @param - f_lane - Target lane
//! @param - car_s - S position of car
//! @param - sensor_fusion - Sensor Fusion Data of all objects
//! @param - num_timestamps_ahead - Number of timestamps ahead to consider for detecting collision
Method which detects whether a collision is possible at N timestamps ahead
Method which detects whether a collision is possible at N timestamps ahead
bool predictCollision(int f_lane, double car_s, vector<vector<double>> sensor_fusion, int num_timestamps_ahead, 
                      vector<double>& map_waypoints_x, vector<double>& map_waypoints_y)
{
  bool isCollidable = false;
  for(int i=0; i<sensor_fusion.size(); i++)
  {
    double d = sensor_fusion[i][6];
    double x = sensor_fusion[i][1];
    double y = sensor_fusion[i][2];
    double x_vel = sensor_fusion[i][3];
    double y_vel = sensor_fusion[i][4];
    double x_future = x + num_timestamps_ahead * 0.02 * x_vel;
    double y_future = y + num_timestamps_ahead * 0.02 * y_vel;
    double yaw = atan2(y_future - y, x_future - x);
    vector<double> other_vehicle_frenet = getFrenet(x_future, y_future, yaw, map_waypoints_x, map_waypoints_y);
    double ego_d = f_lane*4 + 2;
    double ego_d_future = other_vehicle_frenet[1]; 
    if( ((d > (ego_d - 2)) && (d < (ego_d + 2)))  )
    {
      double vx = sensor_fusion[i][3];
      double vy = sensor_fusion[i][4];
      double veh_speed = sqrt(vx * vx + vy * vy);
      double s = sensor_fusion[i][5];
      s += num_timestamps_ahead * 0.02 * veh_speed;
      double s_present = sensor_fusion[i][5];
      double s_future = s;
      if( (s_future > car_s) && ((s_future - car_s) < horizon_front) )
      {
        isCollidable = true;
        break;
      }
    }
  }
  return isCollidable;
}
bool predictCollision ( int f_lane , double car_s , vector < vector < double > > sensor_fusion , int num_timestamps_ahead , vector < double > & map_waypoints_x , vector < double > & map_waypoints_y ) { bool isCollidable = false ; for ( int i = 0 ; i < sensor_fusion . size ( ) ; i ++ ) { double d = sensor_fusion [ i ] [ 6 ] ; double x = sensor_fusion [ i ] [ 1 ] ; double y = sensor_fusion [ i ] [ 2 ] ; double x_vel = sensor_fusion [ i ] [ 3 ] ; double y_vel = sensor_fusion [ i ] [ 4 ] ; double x_future = x + num_timestamps_ahead * 0.02 * x_vel ; double y_future = y + num_timestamps_ahead * 0.02 * y_vel ; double yaw = atan2 ( y_future - y , x_future - x ) ; vector < double > other_vehicle_frenet = getFrenet ( x_future , y_future , yaw , map_waypoints_x , map_waypoints_y ) ; double ego_d = f_lane * 4 + 2 ; double ego_d_future = other_vehicle_frenet [ 1 ] ; if ( ( ( d > ( ego_d - 2 ) ) && ( d < ( ego_d + 2 ) ) ) ) { double vx = sensor_fusion [ i ] [ 3 ] ; double vy = sensor_fusion [ i ] [ 4 ] ; double veh_speed = sqrt ( vx * vx + vy * vy ) ; double s = sensor_fusion [ i ] [ 5 ] ; s += num_timestamps_ahead * 0.02 * veh_speed ; double s_present = sensor_fusion [ i ] [ 5 ] ; double s_future = s ; if ( ( s_future > car_s ) && ( ( s_future - car_s ) < horizon_front ) ) { isCollidable = true ; break ; } } } return isCollidable ; }
Method which detects whether a collision is possible at N timestamps ahead
Method which detects whether a collision is possible at N timestamps ahead
//f_lane*4 + 2; //! Consider only vehicles within the target lane /*|| ((d > (ego_d_future - 2)) && (d < (ego_d_future + 2)))*/ //other_vehicle_frenet[0]; //! Detect collision only if the target is in front of ego vehicle and distance is less than the horizon limit