tune for submission

src/checkcollision.cpp

@@ -28,8 +28,8 @@ int overlap(vector<double> a, vector<double> b) {
 int checkCollision(double s0, double d0, double theta0, double s1, double d1, double theta1) {
   /* IMPLEMENT SEPERATION OF AXIS THEOREM for collision detection */
   // set safety distance (to vehicle heading)
-  double safety_dist_lon = 5.0;
-  double safety_dist_lat = 3.2/2.0;
+  double safety_dist_lon = 5.5;
+  double safety_dist_lat = 3.8/2.0;
 
   // vehicle wrapper
   MatrixXd rec_wrapper(2,4);

src/main.cpp

@@ -180,7 +180,7 @@ int main() {
           	// TODO: define a path made up of (x,y) points that the car will visit sequentially every .02 seconds
 
             double MPH2mps = 1.0/2.23694;
-            double max_speed = 45*MPH2mps;
+            double max_speed = 43*MPH2mps;
 
             int prev_path_size = previous_path_x.size();
             int max_s_waypoint = map_waypoints_s[map_waypoints_s.size()-1];
@@ -202,8 +202,8 @@ int main() {
             int id_map_last = map_waypoints_x.size() - 1;
             int _close_way_point_id = ClosestWaypoint(car_x, car_y, map_waypoints_x, map_waypoints_y);
 
-            int id_interp_start = _close_way_point_id - 4;
-            int id_interp_end   = _close_way_point_id + 7;
+            int id_interp_start = _close_way_point_id - 5;
+            int id_interp_end   = _close_way_point_id + 8;
 
             // double prev_s_offset = s_offset;
             // s_offset = map_waypoints_s[_close_way_point_id];
@@ -264,13 +264,13 @@ int main() {
               map_ss.push_back(_s);
               map_xs.push_back(_x);
               map_ys.push_back(_y);
-              _s += 0.05;
+              _s += 0.1;
             }
 
             // INITIALIZE PLANNER
             vector<Planner> planners;
             for (int i=0; i<3; i++) {
-              double _target_d = 2.0 + 4* i;
+              double _target_d = 2.0 + 4* i - 0.15;
               Planner planner;
               MatrixXd s_trajectories(6, 0);
               VectorXd s_costs(0);
@@ -362,7 +362,7 @@ int main() {
                   }
                   else {planners[j].obstacles.push_back(_vehicle);}
 
-                  if (from_ego_to_other >= -3.0){
+                  if (from_ego_to_other >= -1.0){
                     if (from_ego_to_other < planners[j].dist_to_target) {
                       planners[j].dist_to_target = from_ego_to_other;
                       planners[j].target_to_follow = _vehicle;
@@ -481,7 +481,7 @@ int main() {
 
                 // OPTIMAL TRAJECTORY SELECTION
                 double klon = 1.0;
-                double klat = 0.5;
+                double klat = 1.8;
                 int ns = planners[i].s_costs.size();
                 int nd = planners[i].d_costs.size();
                 int ntraj = ns * nd;
@@ -505,7 +505,7 @@ int main() {
 
                   // collision check
                   // cout << " [*] checking collision " << i+1 << " ..." << endl;
-                  int max_iter = 100;
+                  int max_iter = 200;
                   int iters = -1;
                   if (max_iter >= ntraj) {max_iter = ntraj;}
                   for (int k=0; k<max_iter; k++) {
@@ -517,6 +517,7 @@ int main() {
                     optimal_d_coeff = planners[i].d_trajectories.col(min_d_idx);
 
                     for (int t=0; t<n_planning_horizon; t++) {
+
                       // my position
                       double _s = getPosition(optimal_s_coeff, t*0.02);
                       double _d = getPosition(optimal_d_coeff, t*0.02);
@@ -527,7 +528,7 @@ int main() {
                       // other car's position
                       for (int n=0; n<planners[i].obstacles.size(); n++) {
                         Vehicle other_car = planners[i].obstacles[n];
-                        double _s_other = other_car.s + t * 0.02 * (other_car.speed - 0.2);
+                        double _s_other = other_car.s + t * 0.02 * (other_car.speed - 0.1);
                         double _d_other = other_car.d;
                         int crash = checkCollision(_s, _d, 0, _s_other, _d_other, 0.0);
                         if (crash == 1) {crash_predicted = true; break;}
@@ -601,9 +602,6 @@ int main() {
 
 
                 // loop solution
-                // if ((s < map_s_to_interp[0]) && (map_s_to_interp[map_s_to_interp.size()-1] >= max_s)) {
-                //   s = s + max_s;
-                // }
                 if (s > map_s_to_interp[map_s_to_interp.size()-1]) {
                   // cout << " if 1 " << endl;
                   // minus_max_s = 1;

src/polysolver.cpp

@@ -248,10 +248,10 @@ int VelocityKeepingTrajectories(double s0, double s0dot, double s0ddot, \
   double s1dot, double max_speed, MatrixXd &s_trajectories, VectorXd &s_costs) {
 
     vector<double> ds1dotset;
-    vector<double> ds1dotcand = {-5.0, -3.0, -2.0, -1.0, 0.0, 2.0, 4.0, 6.0, 8.0, 10.0, 12.0, 14.0};
+    vector<double> ds1dotcand = {-3.0, -2.0, -1.0, 0.0, 2.0, 4.0, 6.0, 8.0, 10.0, 12.0};
     vector<double> Tjset = {3.0,3.5,4.0};
     double kspeed = 9.0;
-    double acc_thres = 3.3;
+    double acc_thres = 3.5;
 
     for (int i=0; i<ds1dotcand.size(); i++){
       double _ds1dot = ds1dotcand[i];
@@ -315,17 +315,20 @@ int FollowingTrajectories(double s0, double s0dot, double s0ddot, double s_lv0,
 int lateralTrajectories(double d0, double d0dot, double d0ddot, \
   double d1, bool in_mylane, MatrixXd &d_trajectories, VectorXd &d_costs) {
     vector<double> dd1set;
+    vector<double> Tjset;
     if (in_mylane) {
-      dd1set = {0};
+      dd1set = {-0.6, 0, 0.6};
+      Tjset = {3.5, 3.7};
     }
     else {
-      dd1set = {-1, -0.5, 0, 0.5, 1};
+      dd1set = {-1.2, -0.6, 0, 0.6, 1.2};
+      Tjset = {3.2, 3.5, 3.7};
     }
-    vector<double> Tjset = {3.0, 3.2, 3.5, 3.7};
+
     double max_speed = 15.0;
     double kspeed = 0.0;
     double ks = 2.0;
-    double acc_thres = 4.5;
+    double acc_thres = 4;
 
     int _ = solvePolynomialsFullTerminalCond(d0, d0dot, d0ddot, d1, 0, 0, \
                                              kspeed, ks, max_speed, acc_thres,\