From 7aeb391b81bb4fdb4ce229f8044dbf421ab232cd Mon Sep 17 00:00:00 2001 From: "Ryein C. Goddard" Date: Sun, 1 Oct 2017 22:26:53 -0400 Subject: [PATCH] made further modifications but got it working --- CMakeLists.txt | 9 +- CMakeLists.txt.user | 9 +- src/helpers.h | 156 ----------------- src/main.cpp | 331 +++++++++++++++++++++++-------------- src/plan.h | 289 -------------------------------- src/road.cpp | 234 -------------------------- src/road.h | 60 ------- src/vehicle.cpp | 395 -------------------------------------------- src/vehicle.h | 91 ---------- 9 files changed, 221 insertions(+), 1353 deletions(-) delete mode 100644 src/helpers.h delete mode 100644 src/plan.h delete mode 100644 src/road.cpp delete mode 100644 src/road.h delete mode 100644 src/vehicle.cpp delete mode 100644 src/vehicle.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a14266..bb3e482 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,10 +7,13 @@ add_definitions(-std=c++11) set(CXX_FLAGS "-Wall") set(CMAKE_CXX_FLAGS, "${CXX_FLAGS}") -set(sources src/main.cpp) +include_directories(/usr/include) +link_directories(/usr/lib64) +#target_include_directories(src) +set(sources src/main.cpp) -if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") +if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") include_directories(/usr/local/include) include_directories(/usr/local/opt/openssl/include) @@ -18,7 +21,7 @@ link_directories(/usr/local/lib) link_directories(/usr/local/opt/openssl/lib) link_directories(/usr/local/Cellar/libuv/1.11.0/lib) -endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") +endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") add_executable(path_planning ${sources}) diff --git a/CMakeLists.txt.user b/CMakeLists.txt.user index 1fb62a7..9e1dffa 100644 --- a/CMakeLists.txt.user +++ b/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -344,9 +344,12 @@ path_planning - /home/irenicus/Projects/udacity/CarND-Path-Planning-Project + /home/irenicus/Projects/udacity/CarND-Path-Planning-Project/build 2 - + + PATH=/usr/lib/x86_64-linux-gnu/qt5/bin:/usr/bin:/home/irenicus/.local/bin:/home/irenicus/bin:/home/irenicus/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/usr/include/ + LD_LIBRARY_PATH=/usr/lib64/ + path_planning (disabled) CMakeProjectManager.CMakeRunConfiguration.path_planning diff --git a/src/helpers.h b/src/helpers.h deleted file mode 100644 index 0fc9636..0000000 --- a/src/helpers.h +++ /dev/null @@ -1,156 +0,0 @@ -#ifndef HELPERS_H -#define HELPERS_H - -#include -#include -#include "Eigen-3.3/Eigen/Core" -#include "Eigen-3.3/Eigen/Dense" -#include "Eigen-3.3/Eigen/QR" - -using namespace std; - -// Checks if the SocketIO event has JSON data. -// If there is data the JSON object in string format will be returned, -// else the empty string "" will be returned. -string hasData ( string s ) -{ - auto found_null = s.find ( "null" ); - auto b1 = s.find_first_of ( "[" ); - auto b2 = s.find_first_of ( "}" ); - if ( found_null != string::npos ) { - return ""; - } else if ( b1 != string::npos && b2 != string::npos ) { - return s.substr ( b1, b2 - b1 + 2 ); - } - return ""; -} - -// For converting back and forth between radians and degrees. -constexpr double pi() -{ - return M_PI; -} - -double deg2rad ( double x ) -{ - return x * pi() / 180; -} - -double rad2deg ( double x ) -{ - return x * 180 / pi(); -} - -double distance ( double x1, double y1, double x2, double y2 ) -{ - return sqrt ( ( x2 - x1 ) * ( x2 - x1 ) + ( y2 - y1 ) * ( y2 - y1 ) ); -} - -int ClosestWaypoint ( double x, double y, const vector &maps_x, const vector &maps_y ) -{ - - double closestLen = 100000; //large number - int closestWaypoint = 0; - - for ( int i = 0; i < maps_x.size(); i++ ) { - double map_x = maps_x[i]; - double map_y = maps_y[i]; - double dist = distance ( x, y, map_x, map_y ); - if ( dist < closestLen ) { - closestLen = dist; - closestWaypoint = i; - } - } - - return closestWaypoint; -} - -int NextWaypoint ( double x, double y, double theta, const vector &maps_x, const vector &maps_y ) -{ - - int closestWaypoint = ClosestWaypoint ( x, y, maps_x, maps_y ); - - double map_x = maps_x[closestWaypoint]; - double map_y = maps_y[closestWaypoint]; - - double heading = atan2 ( ( map_y - y ), ( map_x - x ) ); - - double angle = abs ( theta - heading ); - - if ( angle > pi() / 4 ) { - closestWaypoint++; - } - - return closestWaypoint; -} - -// Transform from Cartesian x,y coordinates to Frenet s,d coordinates -vector getFrenet ( double x, double y, double theta, const vector &maps_x, const vector &maps_y ) -{ - int next_wp = NextWaypoint ( x, y, theta, maps_x, maps_y ); - - int prev_wp; - prev_wp = next_wp - 1; - if ( next_wp == 0 ) { - prev_wp = maps_x.size() - 1; - } - - double n_x = maps_x[next_wp] - maps_x[prev_wp]; - double n_y = maps_y[next_wp] - maps_y[prev_wp]; - double x_x = x - maps_x[prev_wp]; - double x_y = y - maps_y[prev_wp]; - - // find the projection of x onto n - double proj_norm = ( x_x * n_x + x_y * n_y ) / ( n_x * n_x + n_y * n_y ); - double proj_x = proj_norm * n_x; - double proj_y = proj_norm * n_y; - - double frenet_d = distance ( x_x, x_y, proj_x, proj_y ); - - //see if d value is positive or negative by comparing it to a center point - - double center_x = 1000 - maps_x[prev_wp]; - double center_y = 2000 - maps_y[prev_wp]; - double centerToPos = distance ( center_x, center_y, x_x, x_y ); - double centerToRef = distance ( center_x, center_y, proj_x, proj_y ); - - if ( centerToPos <= centerToRef ) { - frenet_d *= -1; - } - - // calculate s value - double frenet_s = 0; - for ( int i = 0; i < prev_wp; i++ ) { - frenet_s += distance ( maps_x[i], maps_y[i], maps_x[i + 1], maps_y[i + 1] ); - } - - frenet_s += distance ( 0, 0, proj_x, proj_y ); - - return {frenet_s, frenet_d}; -} - -// Transform from Frenet s,d coordinates to Cartesian x,y -vector getXY ( double s, double d, const vector &maps_s, const vector &maps_x, const vector &maps_y ) -{ - int prev_wp = -1; - - while ( s > maps_s[prev_wp + 1] && ( prev_wp < ( int ) ( maps_s.size() - 1 ) ) ) { - prev_wp++; - } - - int wp2 = ( prev_wp + 1 ) % maps_x.size(); - - double heading = atan2 ( ( maps_y[wp2] - maps_y[prev_wp] ), ( maps_x[wp2] - maps_x[prev_wp] ) ); - // the x,y,s along the segment - double seg_s = ( s - maps_s[prev_wp] ); - - double seg_x = maps_x[prev_wp] + seg_s * cos ( heading ); - double seg_y = maps_y[prev_wp] + seg_s * sin ( heading ); - - double perp_heading = heading - pi() / 2; - - double x = seg_x + d * cos ( perp_heading ); - double y = seg_y + d * sin ( perp_heading ); - - return {x, y}; -} diff --git a/src/main.cpp b/src/main.cpp index f114f4d..826d844 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,10 +12,6 @@ #include "json.hpp" #include "spline.h" -#include "road.h" - -#include "vehicle.h" - using namespace std; // for convenience @@ -179,7 +175,26 @@ int main() // Waypoint map to read from string map_file_ = "../data/highway_map.csv"; // The max s value before wrapping around the track back to 0 - double max_s = 6945.554; + const double max_s = 6945.554; + + // start in lane 1 + int lane = 1; + + // have a reference velocity to target + double ref_vel = 0.0; // mph + const double max_a = 0.224; + + const double speed_limit = 50; + const double buffer_v = 2.0; + const double target_speed = 48; + + const double stop_cost = 0.88; + double speed_cost = 0; + + int target_lane = 1; + bool start_wait = false; + + const double max_s_distance = 30; ifstream in_map_ ( map_file_.c_str(), ifstream::in ); @@ -203,14 +218,22 @@ int main() map_waypoints_dy.push_back ( d_y ); } - // start in lane 1 - int lane = 1; - - // have a reference velocity to target - double ref_vel = 0.0; // mph - - h.onMessage ( [&ref_vel, &lane, &map_waypoints_x, &map_waypoints_y, &map_waypoints_s, &map_waypoints_dx, &map_waypoints_dy] ( uWS::WebSocket ws, char *data, size_t length, - uWS::OpCode opCode ) { + h.onMessage ( [ + &ref_vel, + &lane, + &speed_limit, + &buffer_v, + &target_speed, + &stop_cost, + &speed_cost, + &max_a, + &max_s, + &start_wait, + &target_lane, + &max_s_distance, + &map_waypoints_x, &map_waypoints_y, &map_waypoints_s, &map_waypoints_dx, &map_waypoints_dy + ] ( uWS::WebSocket ws, char *data, size_t length, + uWS::OpCode opCode ) { // "42" at the start of the message means there's a websocket message event. // The 4 signifies a websocket message // The 2 signifies a websocket event @@ -246,15 +269,25 @@ int main() // Sensor Fusion Data, a list of all other cars on the same side of the road. // auto sensor_fusion = j[1]["sensor_fusion"]; vector> sensor_fusion = j[1]["sensor_fusion"]; - + int prev_size = previous_path_x.size(); + // prevent assigning a null value if ( prev_size > 0 ) { car_s = end_path_s; } + // flag for being to close to vehicle ahead bool too_close = false; - + double check_s_difference = 30.0; + double s_difference = 0; + double d_difference = 0; + + double left_front_distance = 9000; + double left_rear_distance = 9000; + double right_front_distance = 9000; + double right_rear_distance = 9000; + // find rev_v to use for ( int i = 0; i < sensor_fusion.size(); i++ ) { //left to right lanes 0, 1, 2 @@ -262,126 +295,184 @@ int main() bool front = false; bool rear = false; bool right = false; - - bool lane_one = true; - bool lane_two = true; - bool lane_three = true; - + double vx = sensor_fusion[i][3]; double vy = sensor_fusion[i][4]; double check_speed = sqrt ( vx * vx + vy * vy ); + + // longitude or y or distance front to back double check_car_s = sensor_fusion[i][5]; - - // car is in my lane + + // latitude or x or lane float d = sensor_fusion[i][6]; - int oc_lane = -1; - for ( int i = 0; i < 3; i++ ) { - if ( d < ( 2 + 4 * i + 2 ) && d > ( 2 + 4 * i - 2 ) ) { - oc_lane = i; - // is the car in i lane ahead of us && is it less then 50 meters - - // need to change this to create zones around car to determine if any where around the car is blocked - // since we don't have some kind of radar this is the best method to avoid accidents i think :D - double safe_distance = distance( car_x, car_y, vx, vy ); - - cout << "distance " << safe_distance << endl; - - if ( safe_distance < (car_x - 200) && safe_distance > (car_x + 200)) { -// if( ( ( sensor_fusion[i][5] - car_s ) > 50 ) ) { //sensor_fusion[i][5] > car_s ) && ( - if ( oc_lane == 0 ) { - lane_three = true; - cout << ", lane 0 " << lane_three; - } - - if ( oc_lane == 1 ) { - lane_two = true; - cout << ", lane 1 " << lane_two; - } - - if ( oc_lane == 2 ) { - lane_one = true; - cout << ", lane 2 " << lane_one; - } - - cout << endl; - - } else { - if ( oc_lane == 0 ) { - lane_three = false; - } - - if ( oc_lane == 1 ) { - lane_two = false; - } - - if ( oc_lane == 2 ) { - lane_one = false; - } - } + + // need to change this to create zones around car to determine if any where around the car is blocked + // since we don't have some kind of radar this is the best method to avoid accidents i think :D + // double safe_distance = helper.distance( car_x, car_y, vx, vy ); + + s_difference = check_car_s - car_s; + d_difference = d - car_d; + + // only check 30 m ahead and 40 behind + // this is some what usable, but without longer memory it will never move to lane 2. + if(s_difference < 30 && s_difference > -40) { + // cout << "distance, s difference, d difference " << safe_distance << ", " << s_difference << ", " << d_difference << endl; + + if(d_difference > 2 && d_difference < 6) { + right = true; + } + + else if(d_difference < -2 && d_difference > -6) { + left = true; + } + + else if(d_difference > -2 && d_difference < 2) { + front = true; + } + + // DEBUG + // if (right) + // cout << "Right" << endl; + + // if (left) + // cout << "Left" << endl; + + // if (front) + // cout << "Front" << endl; + } + + // double lane_calc = ( 2 + 4 * lane + 2 ); + // left of my car lane + if ( lane > 0 ) + { + if ( d < ( 2 + 4 * ( lane - 1 ) + 2 ) && d > ( 2 + 4 * ( lane - 1 ) - 2 ) ) + { + check_car_s += ( ( double ) prev_size * 0.02 * check_speed ); + if (check_car_s > car_s && (check_car_s - car_s) < left_front_distance) + left_front_distance = check_car_s - car_s; + + if (check_car_s < car_s && (car_s - check_car_s) < left_rear_distance) + left_rear_distance = car_s - check_car_s; + } + } + + // right of my car lane + if (lane < 2) + { + if ( d < ( 2 + 4 * ( lane + 1 ) + 2 ) && d > ( 2 + 4 *( lane + 1) - 2 ) ) + { + check_car_s += ( ( double ) prev_size * 0.02 * check_speed ); + if (check_car_s > car_s && (check_car_s - car_s) < right_front_distance) + right_front_distance = check_car_s - car_s; + + if (check_car_s < car_s && (car_s - check_car_s) < right_rear_distance) + right_rear_distance = car_s - check_car_s; } } -// if(car_s < sensor_fusion[i][5]) { -// cout << "lane : " << lane << endl; -// cout << "pos : " << ( 2 + 4 * lane + 2 ) << endl; -// cout << "car : " << car_s << ", " << car_d << endl; -// // cout << "other car : " << sensor_fusion[i][3] << ", " << sensor_fusion[i][4] << endl << endl; -// cout << "other car lane : " << oc_lane << endl; -// cout << "other car : " << sensor_fusion[i][5] << ", " << sensor_fusion[i][6] << endl << endl; -// } - - if ( d < ( 2 + 4 * lane + 2 ) && d > ( 2 + 4 * lane - 2 ) ) { - // if using previous points can project s value out + // cars in my cars current lane + if ( d < ( 2 + 4 * lane + 2 ) && d > ( 2 + 4 * lane - 2 ) ) + { check_car_s += ( ( double ) prev_size * 0.02 * check_speed ); - //check s values greater than mine and s gap - if ( ( check_car_s > car_s ) && ( ( check_car_s - car_s ) < 30 ) ) { - ref_vel -= 0.224; - + // avoid accidents and don't get within max_s_distance which is 30 + if ((check_car_s > car_s) && ((check_car_s - car_s) < max_s_distance )) + { too_close = true; - - cout << "lane, l1, l2, l3 : " << lane << ", " << lane_one << ", " << lane_two << ", " << lane_three << endl << endl; - - // check left lane and see if we can move over - // if not lets move to another - if ( lane == 0 ) { - if ( lane_two ) { - lane = 1; - } - continue; - } - - else if ( lane == 1 ) { - if ( lane_three ) { - lane = 0; - cout << "lane 1 move to 0" << endl; - continue; - } - - else if ( lane_one ){ - lane = 2; - cout << "lane 2 move to 1" << endl; - continue; - } - } - - else if ( lane == 2 ) { - if ( lane_two ) { - lane = 1; - } - continue; - } + check_s_difference = check_car_s - car_s; } } + + // if ( d < ( 2 + 4 * lane + 2 ) && d > ( 2 + 4 * lane - 2 ) ) { + // // if using previous points can project s value out + // check_car_s += ( ( double ) prev_size * 0.02 * check_speed ); + + // //check s values greater than mine and s gap + // if ( ( check_car_s > car_s ) && ( ( check_car_s - car_s ) < 30 ) ) { + + // too_close = true; + + // // check left lane and see if we can move over + // // if not lets move to another + // if ( lane != 0 && !left ) { + // lane--; + // } + + // else if ( lane != 2 && !right) { + // lane++; + // } + // } + // } + } + // speed costs if ( too_close ) { - ref_vel -= 0.224; + ref_vel -= min( 0.8, 10.0 / check_s_difference ); } - else if ( ref_vel < 49.5 ) { - ref_vel += 0.224; + else if ( ref_vel > speed_limit ) { + speed_cost = 1; + ref_vel -= 0.5; + } + + else if ( ref_vel < target_speed ) { + ref_vel += 0.5; + speed_cost = stop_cost * ((target_speed - ref_vel) / target_speed); + } + + else if( ref_vel > target_speed && ref_vel < speed_limit ) { + speed_cost = (ref_vel - target_speed) / buffer_v; + } + + if(check_s_difference < 5.0) + ref_vel -= 1.0; + + // avoid going negative + if (ref_vel < 0.0) + ref_vel = 0.0; + + // Initial start up flag + if ( car_speed > 25.0 && !start_wait && too_close ) + { + start_wait = true; + } + + // find the middle safe area to make lane changes + if (car_speed < 45.0 && check_s_difference < 30.0 && start_wait && check_s_difference > 10.0) + { + // change lanes once again if previous is complete and we are in center + bool lane_change_completed = car_d < ( 2 + 4 * target_lane + 1 ) && car_d > ( 2 + 4 * target_lane - 1 ); + + // no lane changes when we are at origin, or end of s coordinates because it will mess stuff up + if (lane_change_completed && car_s > 50 && car_s < max_s && car_speed > 35) + { + bool turn_left = false; + bool turn_right = false; + + // left lane is open + if ( lane > 0 && left_front_distance > 30 && left_rear_distance>20 ) + turn_left = true; + + // right lane is open + if ( lane < 2 && right_front_distance > 30 && right_rear_distance>20 ) + turn_right = true; + + // if only left open or it has greater distance + if ((turn_left && !turn_right) || ((turn_left && turn_right) && (left_front_distance > right_front_distance))) + { + lane -= 1; + target_lane = lane; + } + + // if only right open or it has greater distance + if ((turn_right && !turn_left) || ((turn_right && turn_left) && (right_front_distance > left_front_distance))) + { + lane += 1; + target_lane = lane; + } + } } vector ptsx; @@ -482,8 +573,6 @@ int main() double x_ref = x_point; double y_ref = y_point; - // cout << x_ref << ", " << y_ref; - // rotate back to normal after rotating it earlier // basis transform? x_point = ( x_ref * cos ( ref_yaw ) - y_ref * sin ( ref_yaw ) ); @@ -497,7 +586,6 @@ int main() } json msgJson; - // TODO: define a path made up of (x,y) points that the car will visit sequentially every .02 seconds msgJson["next_x"] = next_x_vals; msgJson["next_y"] = next_y_vals; @@ -518,7 +606,7 @@ int main() // program // doesn't compile :-( h.onHttpRequest ( [] ( uWS::HttpResponse *res, uWS::HttpRequest req, char *data, - size_t, size_t ) { + size_t, size_t ) { const std::string s = "

Hello world!

"; if ( req.getUrl().valueLength == 1 ) { res->end ( s.data(), s.length() ); @@ -533,7 +621,7 @@ int main() } ); h.onDisconnection ( [&h] ( uWS::WebSocket ws, int code, - char *message, size_t length ) { + char *message, size_t length ) { ws.close(); std::cout << "Disconnected" << std::endl; } ); @@ -547,4 +635,3 @@ int main() } h.run(); } - diff --git a/src/plan.h b/src/plan.h deleted file mode 100644 index 188ecd1..0000000 --- a/src/plan.h +++ /dev/null @@ -1,289 +0,0 @@ -#ifndef PLAN_H -#define PLAN_H - -#include "spline.h" -#include -#include -#include "Eigen-3.3/Eigen/Core" -#include "Eigen-3.3/Eigen/Dense" -#include "Eigen-3.3/Eigen/QR" - -#include "helpers.h" - -using namespace std; -using Eigen::MatrixXd; -using Eigen::VectorXd; - -class Plan { - -public: - double max_s = 6945.554; - - // start in lane 1 - int lane = 1; - - // have a reference velocity to target - double ref_vel = 0.0; // mph - - int prev_size = previous_path_x.size(); - bool too_close = false; - - - Plan() { - - if ( prev_size > 0 ) - { - car_s = end_path_s; - } - - } - -// find rev_v to use - for ( int i = 0; i < sensor_fusion.size(); i++ ) - { - //left to right lanes 0, 1, 2 - bool left = false; - bool front = false; - bool rear = false; - bool right = false; - - bool lane_one = true; - bool lane_two = true; - bool lane_three = true; - - double vx = sensor_fusion[i][3]; - double vy = sensor_fusion[i][4]; - double check_speed = sqrt ( vx * vx + vy * vy ); - double check_car_s = sensor_fusion[i][5]; - - // car is in my lane - float d = sensor_fusion[i][6]; - - int oc_lane = -1; - for ( int i = 0; i < 3; i++ ) { - if ( d < ( 2 + 4 * i + 2 ) && d > ( 2 + 4 * i - 2 ) ) { - oc_lane = i; - // is the car in i lane ahead of us && is it less then 50 meters - - // need to change this to create zones around car to determine if any where around the car is blocked - // since we don't have some kind of radar this is the best method to avoid accidents i think :D - double safe_distance = distance ( car_x, car_y, vx, vy ); - - cout << "distance " << safe_distance << endl; - - if ( safe_distance < ( car_x - 200 ) && safe_distance > ( car_x + 200 ) ) { - // if( ( ( sensor_fusion[i][5] - car_s ) > 50 ) ) { //sensor_fusion[i][5] > car_s ) && ( - if ( oc_lane == 0 ) { - lane_three = true; - cout << ", lane 0 " << lane_three; - } - - if ( oc_lane == 1 ) { - lane_two = true; - cout << ", lane 1 " << lane_two; - } - - if ( oc_lane == 2 ) { - lane_one = true; - cout << ", lane 2 " << lane_one; - } - - cout << endl; - - } else { - if ( oc_lane == 0 ) { - lane_three = false; - } - - if ( oc_lane == 1 ) { - lane_two = false; - } - - if ( oc_lane == 2 ) { - lane_one = false; - } - } - } - } - - // if(car_s < sensor_fusion[i][5]) { - // cout << "lane : " << lane << endl; - // cout << "pos : " << ( 2 + 4 * lane + 2 ) << endl; - // cout << "car : " << car_s << ", " << car_d << endl; - // // cout << "other car : " << sensor_fusion[i][3] << ", " << sensor_fusion[i][4] << endl << endl; - // cout << "other car lane : " << oc_lane << endl; - // cout << "other car : " << sensor_fusion[i][5] << ", " << sensor_fusion[i][6] << endl << endl; - // } - - if ( d < ( 2 + 4 * lane + 2 ) && d > ( 2 + 4 * lane - 2 ) ) { - // if using previous points can project s value out - check_car_s += ( ( double ) prev_size * 0.02 * check_speed ); - - //check s values greater than mine and s gap - if ( ( check_car_s > car_s ) && ( ( check_car_s - car_s ) < 30 ) ) { - ref_vel -= 0.224; - - too_close = true; - - cout << "lane, l1, l2, l3 : " << lane << ", " << lane_one << ", " << lane_two << ", " << lane_three << endl << endl; - - // check left lane and see if we can move over - // if not lets move to another - if ( lane == 0 ) { - if ( lane_two ) { - lane = 1; - } - continue; - } - - else if ( lane == 1 ) { - if ( lane_three ) { - lane = 0; - cout << "lane 1 move to 0" << endl; - continue; - } - - else if ( lane_one ) { - lane = 2; - cout << "lane 2 move to 1" << endl; - continue; - } - } - - else if ( lane == 2 ) { - if ( lane_two ) { - lane = 1; - } - continue; - } - } - } - } - - if ( too_close ) - { - ref_vel -= 0.224; - } - - else if ( ref_vel < 49.5 ) - { - ref_vel += 0.224; - } - - vector ptsx; - vector ptsy; - -// reference x, y, yaw states -// either we will reference the starting point as where -// the car is or at the previous paths end point - double ref_x = car_x; - double ref_y = car_y; - double ref_yaw = deg2rad ( car_yaw ); - -// if previous size is almost empty, use the car as starting reference. - if ( prev_size < 2 ) - { - // use two points that make the path tangent to the car - double prev_car_x = car_x - cos ( car_yaw ); - double prev_car_y = car_y - sin ( car_yaw ); - - ptsx.push_back ( prev_car_x ); - ptsy.push_back ( prev_car_y ); - - ptsx.push_back ( car_x ); - ptsy.push_back ( car_y ); - } - -// use the previous paths end point as starting reference - else - { - // redefine reference state as previous path end point - ref_x = previous_path_x[prev_size - 1]; - ref_y = previous_path_y[prev_size - 1]; - - double ref_x_prev = previous_path_x[prev_size - 2]; - double ref_y_prev = previous_path_y[prev_size - 2]; - ref_yaw = atan2 ( ref_y - ref_y_prev, ref_x - ref_x_prev ); - - // use two points that make the path tangent to the previous paths end point - ptsx.push_back ( ref_x_prev ); - ptsy.push_back ( ref_y_prev ); - - ptsx.push_back ( ref_x ); - ptsy.push_back ( ref_y ); - } - -// In Frenet add evenly 30m space points ahead of the starting reference - vector next_wp0 = getXY ( car_s + 30, ( 2 + 4 * lane ), map_waypoints_s, map_waypoints_x, map_waypoints_y ); - vector next_wp1 = getXY ( car_s + 60, ( 2 + 4 * lane ), map_waypoints_s, map_waypoints_x, map_waypoints_y ); - vector next_wp2 = getXY ( car_s + 90, ( 2 + 4 * lane ), map_waypoints_s, map_waypoints_x, map_waypoints_y ); - - ptsx.push_back ( next_wp0[0] ); - ptsy.push_back ( next_wp0[1] ); - - ptsx.push_back ( next_wp1[0] ); - ptsy.push_back ( next_wp1[1] ); - - ptsx.push_back ( next_wp2[0] ); - ptsy.push_back ( next_wp2[1] ); - - for ( int i = 0; i < ptsx.size(); i++ ) - { - // shift car reference angle to 0 degrees - double shift_x = ptsx[i] - ref_x; - double shift_y = ptsy[i] - ref_y; - - ptsx[i] = ( shift_x * cos ( 0 - ref_yaw ) - shift_y * sin ( 0 - ref_yaw ) ); - ptsy[i] = ( shift_x * sin ( 0 - ref_yaw ) + shift_y * cos ( 0 - ref_yaw ) ); - } - -// create spline - tk::spline s; - -// set(x, y) points to the spline - s.set_points ( ptsx, ptsy ); - -// define the actual (x, y) points we will use for the Planner - vector next_x_vals; - vector next_y_vals; - -// start with all of the previous path points from the last time - for ( int i = 0; i < previous_path_x.size(); i++ ) - { - next_x_vals.push_back ( previous_path_x[i] ); - next_y_vals.push_back ( previous_path_y[i] ); - } - -// Calculate how to break up spline points so that we travel at our desired reference velocity - double target_x = 30.0; - double target_y = s ( target_x ); - double target_dist = sqrt ( ( target_x ) * ( target_x ) + ( target_y ) * ( target_y ) ); - - double x_add_on = 0; - -// Fill up the rest of our path plannner after filling it with previous points here we will output 50 points - for ( int i = 1; i <= 50 - previous_path_x.size(); i++ ) - { - double N = ( target_dist / ( 0.02 * ref_vel / 2.24 ) ); - double x_point = x_add_on + ( target_x ) / N; - double y_point = s ( x_point ); - - x_add_on = x_point; - - double x_ref = x_point; - double y_ref = y_point; - - // cout << x_ref << ", " << y_ref; - - // rotate back to normal after rotating it earlier - // basis transform? - x_point = ( x_ref * cos ( ref_yaw ) - y_ref * sin ( ref_yaw ) ); - y_point = ( x_ref * sin ( ref_yaw ) + y_ref * cos ( ref_yaw ) ); - - x_point += ref_x; - y_point += ref_y; - - next_x_vals.push_back ( x_point ); - next_y_vals.push_back ( y_point ); - } -} diff --git a/src/road.cpp b/src/road.cpp deleted file mode 100644 index 5f624e7..0000000 --- a/src/road.cpp +++ /dev/null @@ -1,234 +0,0 @@ -#include -#include "road.h" -#include "vehicle.h" -#include -#include -#include -#include -#include - - -/** - * Initializes Road - */ -Road::Road ( int speed_limit, double traffic_density, vector lane_speeds ) -{ - - this->num_lanes = lane_speeds.size(); - this->lane_speeds = lane_speeds; - this->speed_limit = speed_limit; - this->density = traffic_density; - this->camera_center = this->update_width/2; - -} - -Road::~Road() {} - -Vehicle Road::get_ego() -{ - - return this->vehicles.find ( this->ego_key )->second; -} - -void Road::populate_traffic() -{ - - int start_s = max ( this->camera_center - ( this->update_width/2 ), 0 ); - for ( int l = 0; l < this->num_lanes; l++ ) { - int lane_speed = this->lane_speeds[l]; - bool vehicle_just_added = false; - for ( int s = start_s; s < start_s+this->update_width; s++ ) { - - if ( vehicle_just_added ) { - vehicle_just_added = false; - } - - if ( ( ( double ) rand() / ( RAND_MAX ) ) < this->density ) { - - Vehicle vehicle = Vehicle ( l,s,lane_speed,0 ); - vehicle.state = "CS"; - this->vehicles_added += 1; - this->vehicles.insert ( std::pair ( vehicles_added,vehicle ) ); - vehicle_just_added = true; - } - } - } - -} - -void Road::advance() -{ - - map > > predictions; - - map::iterator it = this->vehicles.begin(); - while ( it != this->vehicles.end() ) { - int v_id = it->first; - vector > preds = it->second.generate_predictions ( 10 ); - predictions[v_id] = preds; - it++; - } - it = this->vehicles.begin(); - while ( it != this->vehicles.end() ) { - int v_id = it->first; - if ( v_id == ego_key ) { - it->second.update_state ( predictions ); - it->second.realize_state ( predictions ); - } - it->second.increment ( 1 ); - - it++; - } - -} - -void Road::display ( int timestep ) -{ - - Vehicle ego = this->vehicles.find ( this->ego_key )->second; - int s = ego.s; - string state = ego.state; - - this->camera_center = max ( s, this->update_width/2 ); - int s_min = max ( this->camera_center - this->update_width/2, 0 ); - int s_max = s_min + this->update_width; - - vector > road; - - for ( int i = 0; i < this->update_width; i++ ) { - vector road_lane; - for ( int ln = 0; ln < this->num_lanes; ln++ ) { - road_lane.push_back ( " " ); - } - road.push_back ( road_lane ); - - } - - map::iterator it = this->vehicles.begin(); - while ( it != this->vehicles.end() ) { - - int v_id = it->first; - Vehicle v = it->second; - - if ( s_min <= v.s && v.s < s_max ) { - string marker = ""; - if ( v_id == this->ego_key ) { - marker = this->ego_rep; - } else { - - stringstream oss; - stringstream buffer; - buffer << " "; - oss << v_id; - for ( int buffer_i = oss.str().length(); buffer_i < 3; buffer_i++ ) { - buffer << "0"; - - } - buffer << oss.str() << " "; - marker = buffer.str(); - } - road[int ( v.s - s_min )][int ( v.lane )] = marker; - } - it++; - } - ostringstream oss; - oss << "+Meters ======================+ step: " << timestep << endl; - int i = s_min; - for ( int lj = 0; lj < road.size(); lj++ ) { - if ( i%20 ==0 ) { - stringstream buffer; - stringstream dis; - dis << i; - for ( int buffer_i = dis.str().length(); buffer_i < 3; buffer_i++ ) { - buffer << "0"; - } - - oss << buffer.str() << dis.str() << " - "; - } else { - oss << " "; - } - i++; - for ( int li = 0; li < road[0].size(); li++ ) { - oss << "|" << road[lj][li]; - } - oss << "|"; - oss << "\n"; - } - - cout << oss.str(); - -} - -void Road::add_ego ( int lane_num, int s, vector config_data ) -{ - - map::iterator it = this->vehicles.begin(); - while ( it != this->vehicles.end() ) { - int v_id = it->first; - Vehicle v = it->second; - if ( v.lane == lane_num && v.s == s ) { - this->vehicles.erase ( v_id ); - } - it++; - } - Vehicle ego = Vehicle ( lane_num, s, this->lane_speeds[lane_num], 0 ); - ego.configure ( config_data ); - ego.state = "KL"; - this->vehicles.insert ( std::pair ( ego_key,ego ) ); - -} - -void Road::cull() -{ - - - Vehicle ego = this->vehicles.find ( this->ego_key )->second; - int center_s = ego.s; - set> claimed; - - map::iterator it = this->vehicles.begin(); - while ( it != this->vehicles.end() ) { - int v_id = it->first; - Vehicle v = it->second; - vector claim_pair = {v.lane,v.s}; - claimed.insert ( claim_pair ); - it++; - } - it = this->vehicles.begin(); - while ( it != this->vehicles.end() ) { - int v_id = it->first; - Vehicle v = it->second; - if ( ( v.s > ( center_s + this->update_width / 2 ) ) || ( v.s < ( center_s - this->update_width / 2 ) ) ) { - try { - claimed.erase ( {v.lane,v.s} ); - } catch ( const exception& e ) { - continue; - } - this->vehicles.erase ( v_id ); - - bool placed = false; - while ( !placed ) { - int lane_num = rand() % this->num_lanes; - int ds = rand() % 14 + ( this->update_width/2-15 ); - if ( lane_num > this->num_lanes/2 ) { - ds*=-1; - } - int s = center_s + ds; - if ( claimed.find ( {lane_num,s} ) != claimed.end() ) { - placed = true; - int speed = lane_speeds[lane_num]; - Vehicle vehicle = Vehicle ( lane_num, s, speed, 0 ); - this->vehicles_added++; - this->vehicles.insert ( std::pair ( vehicles_added,vehicle ) ); - cout << "adding vehicle "<< this->vehicles_added << " at lane " << lane_num << " with s=" << s << endl; - } - - } - } - it++; - } - - -} - - diff --git a/src/road.h b/src/road.h deleted file mode 100644 index 1b366bf..0000000 --- a/src/road.h +++ /dev/null @@ -1,60 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "vehicle.h" - -using namespace std; - -class Road { -public: - - int update_width = 70; - - string ego_rep = " *** "; - - int ego_key = -1; - - int num_lanes; - - vector lane_speeds; - - int speed_limit; - - double density; - - int camera_center; - - map vehicles; - - int vehicles_added = 0; - - /** - * Constructor - */ - Road(int speed_limit, double traffic_density, vector lane_speeds); - - /** - * Destructor - */ - virtual ~Road(); - - Vehicle get_ego(); - - void populate_traffic(); - - void advance(); - - void display(int timestep); - - void add_ego(int lane_num, int s, vector config_data); - - void cull(); - -}; diff --git a/src/vehicle.cpp b/src/vehicle.cpp deleted file mode 100644 index 8d57aab..0000000 --- a/src/vehicle.cpp +++ /dev/null @@ -1,395 +0,0 @@ -#include -#include "vehicle.h" -#include -#include -#include -#include -#include -#include - -/** - * Initializes Vehicle - */ -Vehicle::Vehicle ( int lane, int s, int v, int a ) -{ - - this->lane = lane; - this->s = s; - this->v = v; - this->a = a; - state = "CS"; - max_acceleration = -1; - -} - -Vehicle::~Vehicle() {} - -void print_pred ( map > > predictions ) -{ - for ( auto const& el: predictions ) { - cout << "Car " << el.first << ": "; - for ( auto const& vec: el.second ) { - for ( auto const& s: vec ) { - cout << s << ","; - } - cout << " - "; - } - cout << endl; - } -} - -float cost_for_action ( map > > predictions, string action, int current_lane, int target_lane, int available_lanes, int target_speed ) -{ - map front_gaps; - for ( int l=0; l::max(); - } - int s = predictions[-1][0][1]; - for ( auto const& entry: predictions ) { - auto vehicle_id = entry.first; - int vl = entry.second[0][0], vs = entry.second[0][1]; - int gap = vs - s; - if ( vehicle_id != -1 && gap > 0 && front_gaps[vl] > gap ) { - front_gaps[vl] = gap; - } - } - - int diff_lane = current_lane - target_lane; - if ( action == "KL" ) { - float diff_lane_cost = abs ( diff_lane ); - float front_gap_cost = target_speed / front_gaps[current_lane]; - return diff_lane_cost + 1.5*front_gap_cost; - } - if ( action == "PLCL" ) { - if ( diff_lane+1 > available_lanes ) { - return numeric_limits::max(); - } - float diff_lane_cost = abs ( diff_lane+1 ); - float front_gap_cost = target_speed / front_gaps[current_lane + 1]; - return diff_lane_cost + 1.5*front_gap_cost; - } - if ( action == "PLCR" ) { - if ( diff_lane-1 < 0 ) { - return numeric_limits::max(); - } - float diff_lane_cost = abs ( diff_lane-1 ); - float front_gap_cost = target_speed / front_gaps[current_lane - 1]; - return diff_lane_cost + 1.5*front_gap_cost; - } - if ( action == "LCL" ) { - if ( diff_lane+1 > available_lanes ) { - return numeric_limits::max(); - } - float diff_lane_cost = abs ( diff_lane+1 ); - float front_gap_cost = target_speed / front_gaps[current_lane + 1]; - return diff_lane_cost + 1.5*front_gap_cost; - } - if ( action == "LCR" ) { - if ( diff_lane-1 < 0 ) { - return numeric_limits::max(); - } - float diff_lane_cost = abs ( diff_lane-1 ); - float front_gap_cost = target_speed / front_gaps[current_lane - 1]; - return diff_lane_cost + 1.5*front_gap_cost; - } - return numeric_limits::max(); -} - -// TODO - Implement this method. -void Vehicle::update_state ( map > > predictions ) -{ - /* - Updates the "state" of the vehicle by assigning one of the - following values to 'self.state': - - "KL" - Keep Lane - - The vehicle will attempt to drive its target speed, unless there is - traffic in front of it, in which case it will slow down. - - "LCL" or "LCR" - Lane Change Left / Right - - The vehicle will IMMEDIATELY change lanes and then follow longitudinal - behavior for the "KL" state in the new lane. - - "PLCL" or "PLCR" - Prepare for Lane Change Left / Right - - The vehicle will find the nearest vehicle in the adjacent lane which is - BEHIND itself and will adjust speed to try to get behind that vehicle. - - INPUTS - - predictions - A dictionary. The keys are ids of other vehicles and the values are arrays - where each entry corresponds to the vehicle's predicted location at the - corresponding timestep. The FIRST element in the array gives the vehicle's - current position. Example (showing a car with id 3 moving at 2 m/s): - - { - 3 : [ - {"s" : 4, "lane": 0}, - {"s" : 6, "lane": 0}, - {"s" : 8, "lane": 0}, - {"s" : 10, "lane": 0}, - ] - } - - */ - - int current_lane = predictions[-1][0][0], target_lane = 0; - vector possible_actions; - if ( state == "KL" ) { - possible_actions = { "KL", "PLCL", "PLCR" }; - } - if ( state == "PLCL" ) { - possible_actions = { "LCL", "KL" }; - } - if ( state == "PLCR" ) { - possible_actions = { "LCR", "KL" }; - } - if ( state == "LCL" ) { - possible_actions = { "PLCL", "KL" }; - } - if ( state == "LCR" ) { - possible_actions = { "PLCR", "KL" }; - } - - vector costs; - for ( auto const& a: possible_actions ) costs.push_back ( - cost_for_action ( predictions, a, current_lane, target_lane, lanes_available, target_speed ) ); - // cout << "Actions : "; - // for (auto const& a: possible_actions) cout << a << ", " ; - // cout << endl << "Costs: "; - // for (auto const& c: costs) cout << c << ", " ; - - int min_pos = distance ( costs.begin(), min_element ( costs.begin(), costs.end() ) ); - // cout << "min_pos " << min_pos << endl; - state = possible_actions[min_pos]; -} - -void Vehicle::configure ( vector road_data ) -{ - /* - Called by simulator before simulation begins. Sets various - parameters which will impact the ego vehicle. - */ - target_speed = road_data[0]; - lanes_available = road_data[1]; - goal_s = road_data[2]; - goal_lane = road_data[3]; - max_acceleration = road_data[4]; -} - -string Vehicle::display() -{ - - ostringstream oss; - - oss << "s: " << this->s << "\n"; - oss << "lane: " << this->lane << "\n"; - oss << "v: " << this->v << "\n"; - oss << "a: " << this->a << "\n"; - - return oss.str(); -} - -void Vehicle::increment ( int dt = 1 ) -{ - - this->s += this->v * dt; - this->v += this->a * dt; -} - -vector Vehicle::state_at ( int t ) -{ - - /* - Predicts state of vehicle in t seconds (assuming constant acceleration) - */ - int s = this->s + this->v * t + this->a * t * t / 2; - int v = this->v + this->a * t; - return {this->lane, s, v, this->a}; -} - -bool Vehicle::collides_with ( Vehicle other, int at_time ) -{ - - /* - Simple collision detection. - */ - vector check1 = state_at ( at_time ); - vector check2 = other.state_at ( at_time ); - return ( check1[0] == check2[0] ) && ( abs ( check1[1]-check2[1] ) <= L ); -} - -Vehicle::collider Vehicle::will_collide_with ( Vehicle other, int timesteps ) -{ - - Vehicle::collider collider_temp; - collider_temp.collision = false; - collider_temp.time = -1; - - for ( int t = 0; t < timesteps+1; t++ ) { - if ( collides_with ( other, t ) ) { - collider_temp.collision = true; - collider_temp.time = t; - return collider_temp; - } - } - - return collider_temp; -} - -void Vehicle::realize_state ( map > > predictions ) -{ - - /* - Given a state, realize it by adjusting acceleration and lane. - Note - lane changes happen instantaneously. - */ - string state = this->state; - if ( state.compare ( "CS" ) == 0 ) { - realize_constant_speed(); - } else if ( state.compare ( "KL" ) == 0 ) { - realize_keep_lane ( predictions ); - } else if ( state.compare ( "LCL" ) == 0 ) { - realize_lane_change ( predictions, "L" ); - } else if ( state.compare ( "LCR" ) == 0 ) { - realize_lane_change ( predictions, "R" ); - } else if ( state.compare ( "PLCL" ) == 0 ) { - realize_prep_lane_change ( predictions, "L" ); - } else if ( state.compare ( "PLCR" ) == 0 ) { - realize_prep_lane_change ( predictions, "R" ); - } - -} - -void Vehicle::realize_constant_speed() -{ - a = 0; -} - -int Vehicle::_max_accel_for_lane ( map > > predictions, int lane, int s ) -{ - - int delta_v_til_target = target_speed - v; - int max_acc = min ( max_acceleration, delta_v_til_target ); - - map > >::iterator it = predictions.begin(); - vector > > in_front; - while ( it != predictions.end() ) { - - int v_id = it->first; - - vector > v = it->second; - - if ( ( v[0][0] == lane ) && ( v[0][1] > s ) ) { - in_front.push_back ( v ); - - } - it++; - } - - if ( in_front.size() > 0 ) { - int min_s = 1000; - vector> leading = {}; - for ( int i = 0; i < in_front.size(); i++ ) { - if ( ( in_front[i][0][1]-s ) < min_s ) { - min_s = ( in_front[i][0][1]-s ); - leading = in_front[i]; - } - } - - int next_pos = leading[1][1]; - int my_next = s + this->v; - int separation_next = next_pos - my_next; - int available_room = separation_next - preferred_buffer; - max_acc = min ( max_acc, available_room ); - } - - return max_acc; - -} - -void Vehicle::realize_keep_lane ( map > > predictions ) -{ - this->a = _max_accel_for_lane ( predictions, this->lane, this->s ); -} - -void Vehicle::realize_lane_change ( map > > predictions, string direction ) -{ - int delta = -1; - if ( direction.compare ( "L" ) == 0 ) { - delta = 1; - } - this->lane += delta; - int lane = this->lane; - int s = this->s; - this->a = _max_accel_for_lane ( predictions, lane, s ); -} - -void Vehicle::realize_prep_lane_change ( map > > predictions, string direction ) -{ - int delta = -1; - if ( direction.compare ( "L" ) == 0 ) { - delta = 1; - } - int lane = this->lane + delta; - - map > >::iterator it = predictions.begin(); - vector > > at_behind; - while ( it != predictions.end() ) { - int v_id = it->first; - vector > v = it->second; - - if ( ( v[0][0] == lane ) && ( v[0][1] <= this->s ) ) { - at_behind.push_back ( v ); - - } - it++; - } - if ( at_behind.size() > 0 ) { - - int max_s = -1000; - vector > nearest_behind = {}; - for ( int i = 0; i < at_behind.size(); i++ ) { - if ( ( at_behind[i][0][1] ) > max_s ) { - max_s = at_behind[i][0][1]; - nearest_behind = at_behind[i]; - } - } - int target_vel = nearest_behind[1][1] - nearest_behind[0][1]; - int delta_v = this->v - target_vel; - int delta_s = this->s - nearest_behind[0][1]; - if ( delta_v != 0 ) { - - int time = -2 * delta_s/delta_v; - int a; - if ( time == 0 ) { - a = this->a; - } else { - a = delta_v/time; - } - if ( a > this->max_acceleration ) { - a = this->max_acceleration; - } - if ( a < -this->max_acceleration ) { - a = -this->max_acceleration; - } - this->a = a; - } else { - int my_min_acc = max ( -this->max_acceleration,-delta_s ); - this->a = my_min_acc; - } - - } -} - -vector > Vehicle::generate_predictions ( int horizon = 10 ) -{ - - vector > predictions; - for ( int i = 0; i < horizon; i++ ) { - vector check1 = state_at ( i ); - vector lane_s = {check1[0], check1[1]}; - predictions.push_back ( lane_s ); - } - return predictions; - -} diff --git a/src/vehicle.h b/src/vehicle.h deleted file mode 100644 index 8a38c91..0000000 --- a/src/vehicle.h +++ /dev/null @@ -1,91 +0,0 @@ -#ifndef VEHICLE_H -#define VEHICLE_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; - -class Vehicle -{ -public: - - struct collider { - - bool collision ; // is there a collision? - int time; // time collision happens - - }; - - int L = 1; - - int preferred_buffer = 6; // impacts "keep lane" behavior. - - int lane; - - int s; - - int v; - - int a; - - int target_speed; - - int lanes_available; - - int max_acceleration; - - int goal_lane; - - int goal_s; - - string state; - - /** - * Constructor - */ - Vehicle ( int lane, int s, int v, int a ); - - /** - * Destructor - */ - virtual ~Vehicle(); - - void update_state ( map > > predictions ); - - void configure ( vector road_data ); - - string display(); - - void increment ( int dt ); - - vector state_at ( int t ); - - bool collides_with ( Vehicle other, int at_time ); - - collider will_collide_with ( Vehicle other, int timesteps ); - - void realize_state ( map > > predictions ); - - void realize_constant_speed(); - - int _max_accel_for_lane ( map > > predictions, int lane, int s ); - - void realize_keep_lane ( map > > predictions ); - - void realize_lane_change ( map > > predictions, string direction ); - - void realize_prep_lane_change ( map > > predictions, string direction ); - - vector > generate_predictions ( int horizon ); - -}; - -#endif