From 66045ecd97bd33d83944d12d0352311cd411380c Mon Sep 17 00:00:00 2001 From: ndrplz Date: Fri, 18 Aug 2017 18:18:28 +0200 Subject: [PATCH] Implement checks before lane change --- project_11_path_planning/src/main.cpp | 60 ++++++++++++++++++--------- project_11_path_planning/src/utils.h | 27 ++++++++++-- 2 files changed, 65 insertions(+), 22 deletions(-) diff --git a/project_11_path_planning/src/main.cpp b/project_11_path_planning/src/main.cpp index b77605b..775c58e 100644 --- a/project_11_path_planning/src/main.cpp +++ b/project_11_path_planning/src/main.cpp @@ -5,7 +5,6 @@ #include #include "Eigen-3.3/Eigen/Core" #include "Eigen-3.3/Eigen/QR" -#include "json.hpp" #include "spline.h" #include "coords_transform.h" @@ -86,7 +85,7 @@ int main() { auto s = hasData(data); if (s != "") { - auto j = json::parse(s); + json j = json::parse(s); string event = j[0].get(); @@ -117,33 +116,56 @@ int main() { if (prev_size > 0) car_s = end_path_s; - bool too_close = false; + bool is_too_close = false; + bool is_left_lane_free = true; + bool is_right_lane_free = true; for (size_t i = 0; i < sensor_fusion.size(); ++i) { // Check if the `i_th` car is on our lane - double d = sensor_fusion[i][6]; - bool is_in_our_lane = (d > lane_width * lane) && (d < lane_width * lane + lane_width); - if (is_in_our_lane) { - double vx = sensor_fusion[i][3]; - double vy = sensor_fusion[i][4]; - double check_speed = sqrt(vx * vx + vy * vy); // speed module - double check_car_s = sensor_fusion[i][5]; - check_car_s += (double)prev_size * 0.02 * check_speed; // use previous points to project s value outward - - bool is_in_front_of_us = check_car_s > car_s; - bool is_closer_than_safety_margin = check_car_s - car_s < safety_margin; + Vehicle vehicle(sensor_fusion[i]); + + if (is_in_lane(vehicle.d, lane)) { + + vehicle.s += (double)prev_size * 0.02 * vehicle.speed; // use previous points to project s value onward + + bool is_in_front_of_us = vehicle.s > car_s; + bool is_closer_than_safety_margin = vehicle.s - car_s < safety_margin; if (is_in_front_of_us && is_closer_than_safety_margin) { - // add the logic here, e.g. either decrease speed or prepare for lane change - too_close = true; - if (lane > 0) - lane = 0; + + is_too_close = true; + + // Check if left and right lanes are free + for (size_t i = 0; i < sensor_fusion.size(); ++i) { + Vehicle vehicle(sensor_fusion[i]); + // Check left lane + if (is_in_lane(vehicle.d, lane - 1)) { + vehicle.s += (double)prev_size * 0.02 * vehicle.speed; + bool too_close_to_change = (vehicle.s > car_s - safety_margin / 2) && (vehicle.s < car_s + safety_margin / 2); + if (too_close_to_change) + is_left_lane_free = false; + } + // Check right lane + else if (is_in_lane(vehicle.d, lane + 1)) { + vehicle.s += (double)prev_size * 0.02 * vehicle.speed; + bool too_close_to_change = (vehicle.s > car_s - safety_margin / 2) && (vehicle.s < car_s + safety_margin / 2); + if (too_close_to_change) + is_right_lane_free = false; + } + } + + // Actually perform lane change + if (lane > 0 && is_left_lane_free) + lane -= 1; + else if (lane < 2 && is_right_lane_free) + lane += 1; } } } - if (too_close) + // Eventually slow down if too close the car before + if (is_too_close) ref_vel -= 0.224; // deceleration around 5 m/s^2 else if (ref_vel < max_safe_speed) ref_vel += 0.224; diff --git a/project_11_path_planning/src/utils.h b/project_11_path_planning/src/utils.h index 4fc69f1..a2c4e9f 100644 --- a/project_11_path_planning/src/utils.h +++ b/project_11_path_planning/src/utils.h @@ -4,12 +4,28 @@ #include #include #include +#include "json.hpp" const double pi = M_PI; -const double lane_width = 4.0; // width of a lane (meters) -const double safety_margin = 30.0; // distance to keep from other cars -const double max_safe_speed = 49.5; // max reference speed in the limit (mph) +const double lane_width = 4.0; // width of a lane (m) +const double safety_margin = 20.0; // distance to keep from other cars (m) +const double max_safe_speed = 49.5; // max reference speed in the limit (mph) + + +struct Vehicle { + double d; + double vx, vy; + double speed; + double s; + Vehicle(nlohmann::json sensor_fusion) { + this->vx = sensor_fusion[3]; + this->vy = sensor_fusion[4]; + this->s = sensor_fusion[5]; + this->d = sensor_fusion[6]; + this->speed = sqrt(vx * vx + vy * vy); + } +}; // For converting back and forth between radians and degrees. double deg2rad(double x) { return x * pi / 180; } @@ -20,4 +36,9 @@ double distance(double x1, double y1, double x2, double y2) { return sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1)); } +// Check if a vehicle is is a certain lane +bool is_in_lane(double d, int lane) { + return (d > lane_width * lane) && (d < lane_width * lane + lane_width); +} + #endif \ No newline at end of file