From a24d7929e63c1becd0e47a6fdce58fcd29b31d0d Mon Sep 17 00:00:00 2001 From: ndrplz Date: Wed, 16 Aug 2017 18:47:02 +0200 Subject: [PATCH] Implement simple collision detection --- project_11_path_planning/src/main.cpp | 31 ++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/project_11_path_planning/src/main.cpp b/project_11_path_planning/src/main.cpp index e551792..17e2fe9 100644 --- a/project_11_path_planning/src/main.cpp +++ b/project_11_path_planning/src/main.cpp @@ -195,7 +195,7 @@ int main() { int lane = 1; // Reference velocity (mph) - double ref_vel = 49.5; + double ref_vel = 0.0; h.onMessage([&map_waypoints_x,&map_waypoints_y,&map_waypoints_s,&map_waypoints_dx,&map_waypoints_dy,&ref_vel, &lane](uWS::WebSocket ws, char *data, size_t length, uWS::OpCode opCode) { @@ -236,6 +236,35 @@ int main() { int prev_size = previous_path_x.size(); + if (prev_size > 0) + car_s = end_path_s; + + bool too_close = false; + + 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]; + if (d < (2 + 4 * lane + 2) && d >(2 + 4 * lane - 2)) { + 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]; + + // use previous points to project s value outward + check_car_s += (double)prev_size * 0.02 * check_speed; + + if (check_car_s > car_s && (check_car_s - car_s) < 30) + // add the logic here, for example either decrease speed or prepare for lane change + too_close = true; + } + } + + if (too_close) + ref_vel -= 0.224; // around 5 m/s^2 + else if (ref_vel < 49.5) + ref_vel += 0.224; + // List of widely spaced (x, y) waypoints. These will be later interpolated with a spline, // filling it with more points which control speed. vector pts_x;