From c4d0c4edd1e00e33570a62b0b76aa2cdc5ee54dd Mon Sep 17 00:00:00 2001
From: Hayoung-Kim <altairyoung@gmail.com>
Date: Sat, 2 Dec 2017 01:45:38 +0900
Subject: [PATCH] select the optimal trajectory where no collision occurs with
 other vehicles

---
 CMakeLists.txt         |  2 +-
 src/checkcollision.cpp | 12 ++++++++++++
 src/checkcollision.h   | 10 ++++++++++
 src/main.cpp           | 37 ++++++++++++++++++++++++++++++++-----
 4 files changed, 55 insertions(+), 6 deletions(-)
 create mode 100644 src/checkcollision.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f52d842..5da1045 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
 set(CXX_FLAGS "-Wall")
 set(CMAKE_CXX_FLAGS, "${CXX_FLAGS}")
 
-set(sources src/main.cpp src/polysolver.cpp src/coordinate_transforms.cpp)
+set(sources src/main.cpp src/polysolver.cpp src/coordinate_transforms.cpp src/checkcollision.cpp)
 
 
 if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 
diff --git a/src/checkcollision.cpp b/src/checkcollision.cpp
index 519e514..4003b2b 100644
--- a/src/checkcollision.cpp
+++ b/src/checkcollision.cpp
@@ -1,4 +1,16 @@
 
+#include <math.h>
+#include <iostream>
+#include <vector>
+
+#include "Eigen-3.3/Eigen/Core"
+#include "Eigen-3.3/Eigen/QR"
+#include "Eigen-3.3/Eigen/Dense"
+
+using namespace std;
+using namespace Eigen;
+#include "checkcollision.h"
+
 int contains(double n, vector<double> range) {
   float a = range[0], b = range[1];
   if (b<a) {a=b; b=range[0];}
diff --git a/src/checkcollision.h b/src/checkcollision.h
new file mode 100644
index 0000000..c4cc599
--- /dev/null
+++ b/src/checkcollision.h
@@ -0,0 +1,10 @@
+#ifndef _CHECKCOLLISION_H_
+#define _CHECKCOLLISION_H_
+
+int contains(double n, vector<double> range);
+
+int overlap(vector<double> a, vector<double> b);
+
+int checkCollision(double s0, double d0, double theta0, double s1, double d1, double theta1);
+
+#endif
diff --git a/src/main.cpp b/src/main.cpp
index 3387f87..0979e12 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -20,6 +20,7 @@ using json = nlohmann::json;
 #include "spline.h"
 #include "polysolver.h"
 #include "coordinate_transforms.h"
+#include "checkcollision.h"
 
 typedef struct Vehicle {
   int id;
@@ -30,6 +31,7 @@ typedef struct Vehicle {
   double s;
   double d;
   double speed;
+  // vector<double> future_s;
 } Vehicle;
 
 // Checks if the SocketIO event has JSON data.
@@ -311,7 +313,7 @@ int main() {
               int n_subset = n_use_previous_path;
               if (n_subset >= prev_path_size) {n_subset = prev_path_size;}
 
-              cout << "n_subset: " << n_subset << endl;
+              // cout << "n_subset: " << n_subset << endl;
 
               for (int h=0; h<n_subset; h++){
                 next_x_vals.push_back(previous_path_x[h]);
@@ -354,13 +356,38 @@ int main() {
               _ = lateralTrajectories(d0, d0dot, d0ddot, 6.0, d_trajectories, d_costs);
               _ = lateralTrajectories(d0, d0dot, d0ddot, 10.0, d_trajectories, d_costs);
 
-              // SELECT PATH
 
+              // OPTIMAL PATH SELECTION WITH COLLISION CHECK
+              int MAX_ITER_FIND_OPT = 40;
+              for (int i=0; i<MAX_ITER_FIND_OPT; i++) {
+                bool is_crash = false;
+                vector<int> opt_idx = optimalCombination(s_costs, d_costs);
+                optimal_s_coeff = s_trajectories.col(opt_idx[0]);
+                optimal_d_coeff = d_trajectories.col(opt_idx[1]);
+
+                // check collision for hrz
+                for (int t=0; t< n_planning_horizon; t++) {
+                  // my position
+                  double _s = getPosition(optimal_s_coeff, i*0.02);
+                  double _d = getPosition(optimal_d_coeff, i*0.02);
+
+                  // other car's predicted position
+                  for (int n=0; n < NearbyVehicles.size(); n++) {
+                    Vehicle other_car = NearbyVehicles[n];
+                    double _s_other = other_car.s + t * 0.02 * other_car.speed;
+                    double _d_other = other_car.d;
+                    int crash = checkCollision(_s, _d, 0.0, _s_other, _d_other, 0.0);
+                    if (crash == 1) {is_crash = true; break;}
+                  }
+                  if (is_crash) {break;}
+                }
+                if (is_crash) {s_costs(opt_idx[0]) = 99999; d_costs(opt_idx[1])=99999;}
+              }
 
 
-              vector<int> opt_idx = optimalCombination(s_costs, d_costs);
-              optimal_s_coeff = s_trajectories.col(opt_idx[0]);
-              optimal_d_coeff = d_trajectories.col(opt_idx[1]);
+              // vector<int> opt_idx = optimalCombination(s_costs, d_costs);
+              // optimal_s_coeff = s_trajectories.col(opt_idx[0]);
+              // optimal_d_coeff = d_trajectories.col(opt_idx[1]);
 
               // RUN!!