src/prediction.cpp
// Check a trajectory candidate for collision risk with other cars
Check a trajectory candidate for collision risk with other cars
Check a trajectory candidate for collision risk with other cars
bool Prediction::IsTrajectoryCollisionFree(TrajectoryCandidate &tc) {
  log_.of_ << "==== Prediction::IsTrajectoryCollisionFree ====" << endl;
  bool front = true, back = true;
  int lane = static_cast<int>(tc.t.lane);
  XYPoints front_xy = predictions_[lane].front_jmt.trajectory.pts;
  XYPoints back_xy = predictions_[lane].back_jmt.trajectory.pts;
  log_.of_ << "Check lane = " << lane << endl;
  if (predictions_[lane].front_valid) {
    front = checkIndividualOtherCar(front_xy, tc);
    if (front)
      log_.of_ << "Check lane = " << lane << " font: no collision" << endl;
    else
      log_.of_ << "Check lane = " << lane << " font: COLLISION" << endl;
  }
  else
  {
    log_.of_ << "Check lane = " << lane << " NO TRAJ DATA FOR FRONT" << endl;
  }
  if (predictions_[lane].back_valid) {
    back = checkIndividualOtherCar(back_xy, tc);
    if (back)
      log_.of_ << "Check lane = " << lane << " back: no collision" << endl;
    else
      log_.of_ << "Check lane = " << lane << " back: COLLISION" << endl;
  }
  else
  {
    log_.of_ << "Check lane = " << lane << " NO TRAJ DATA FOR BACK" << endl;
  }
  if (front && back)
    log_.of_ << "No collision detected" << endl;
  else
    log_.of_ << "Collision detected" << endl;
  log_.of_ << "==== ****** ====" << endl;
  return (front && back);
}
bool Prediction :: IsTrajectoryCollisionFree ( TrajectoryCandidate & tc ) { log_ . of_ << " " << endl ; bool front = true , back = true ; int lane = static_cast < int > ( tc . t . lane ) ; XYPoints front_xy = predictions_ [ lane ] . front_jmt . trajectory . pts ; XYPoints back_xy = predictions_ [ lane ] . back_jmt . trajectory . pts ; log_ . of_ << " " << lane << endl ; if ( predictions_ [ lane ] . front_valid ) { front = checkIndividualOtherCar ( front_xy , tc ) ; if ( front ) log_ . of_ << " " << lane << " " << endl ; else log_ . of_ << " " << lane << " " << endl ; } else { log_ . of_ << " " << lane << " " << endl ; } if ( predictions_ [ lane ] . back_valid ) { back = checkIndividualOtherCar ( back_xy , tc ) ; if ( back ) log_ . of_ << " " << lane << " " << endl ; else log_ . of_ << " " << lane << " " << endl ; } else { log_ . of_ << " " << lane << " " << endl ; } if ( front && back ) log_ . of_ << " " << endl ; else log_ . of_ << " " << endl ; log_ . of_ << " " << endl ; return ( front && back ) ; }
Check a trajectory candidate for collision risk with other cars
Check a trajectory candidate for collision risk with other cars
/* log_.of_ << "front x size = " << front_xy.xs.size() << endl;
  log_.of_ << "back x size = " << back_xy.xs.size() << endl; */ // *** REMOVE