diff --git a/project_11_path_planning/src/main.cpp b/project_11_path_planning/src/main.cpp index 872426d..b77605b 100644 --- a/project_11_path_planning/src/main.cpp +++ b/project_11_path_planning/src/main.cpp @@ -7,7 +7,6 @@ #include "Eigen-3.3/Eigen/QR" #include "json.hpp" #include "spline.h" -#include "utils.h" #include "coords_transform.h" @@ -34,7 +33,7 @@ string hasData(string s) { int main() { uWS::Hub h; - // Load up map values for waypoint's x,y,s and d normalized normal vectors + // Load up map values for waypoint's `x`, `y`, `s` and `d` normalized normal vectors vector map_waypoints_x; vector map_waypoints_y; vector map_waypoints_s; @@ -43,6 +42,7 @@ 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; @@ -93,22 +93,23 @@ int main() { if (event == "telemetry") { // j[1] is the data JSON object - // Main car's localization Data - double car_x = j[1]["x"]; - double car_y = j[1]["y"]; - double car_s = j[1]["s"]; - double car_d = j[1]["d"]; - double car_yaw = j[1]["yaw"]; + // Main car's localization data + double car_x = j[1]["x"]; + double car_y = j[1]["y"]; + double car_s = j[1]["s"]; + double car_d = j[1]["d"]; + double car_yaw = j[1]["yaw"]; double car_speed = j[1]["speed"]; // Previous path data given to the Planner auto previous_path_x = j[1]["previous_path_x"]; auto previous_path_y = j[1]["previous_path_y"]; - // Previous path's end s and d values + + // Previous path's end `s` and `d` values double end_path_s = j[1]["end_path_s"]; double end_path_d = j[1]["end_path_d"]; - // Sensor Fusion Data, a list of all other cars on the same side of the road. + // Sensor Fusion data - a list of all other cars on the same side of the road. auto sensor_fusion = j[1]["sensor_fusion"]; int prev_size = previous_path_x.size(); @@ -122,17 +123,19 @@ int main() { // 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)) { + 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 - // use previous points to project s value outward - check_car_s += (double)prev_size * 0.02 * check_speed; + bool is_in_front_of_us = check_car_s > car_s; + bool is_closer_than_safety_margin = check_car_s - car_s < safety_margin; - 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 + 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; @@ -141,12 +144,12 @@ int main() { } if (too_close) - ref_vel -= 0.224; // around 5 m/s^2 - else if (ref_vel < 49.5) + ref_vel -= 0.224; // deceleration around 5 m/s^2 + else if (ref_vel < max_safe_speed) 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. + // 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; vector pts_y; @@ -177,9 +180,9 @@ int main() { } // In Frenet coordinates, add evenly 30m spaced points ahead of the starting reference - vector next_wp0 = frenet_to_cartesian(car_s + 30, (2 + 4 * lane), map_waypoints_s, map_waypoints_x, map_waypoints_y); - vector next_wp1 = frenet_to_cartesian(car_s + 60, (2 + 4 * lane), map_waypoints_s, map_waypoints_x, map_waypoints_y); - vector next_wp2 = frenet_to_cartesian(car_s + 90, (2 + 4 * lane), map_waypoints_s, map_waypoints_x, map_waypoints_y); + vector next_wp0 = frenet_to_cartesian(car_s + 30, (lane_width * lane + lane_width / 2), map_waypoints_s, map_waypoints_x, map_waypoints_y); + vector next_wp1 = frenet_to_cartesian(car_s + 60, (lane_width * lane + lane_width / 2), map_waypoints_s, map_waypoints_x, map_waypoints_y); + vector next_wp2 = frenet_to_cartesian(car_s + 90, (lane_width * lane + lane_width / 2), map_waypoints_s, map_waypoints_x, map_waypoints_y); pts_x.push_back(next_wp0[0]); pts_x.push_back(next_wp1[0]); pts_x.push_back(next_wp2[0]); pts_y.push_back(next_wp0[1]); pts_y.push_back(next_wp1[1]); pts_y.push_back(next_wp2[1]); @@ -235,15 +238,12 @@ int main() { next_y_vals.push_back(y_point); } + // Prepare message and send it to the simulator json msgJson; msgJson["next_x"] = next_x_vals; msgJson["next_y"] = next_y_vals; - auto msg = "42[\"control\","+ msgJson.dump()+"]"; - - //this_thread::sleep_for(chrono::milliseconds(1000)); ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT); - } } else { // Manual driving @@ -254,8 +254,7 @@ int main() { }); // We don't need this since we're not using HTTP but if it's removed the - // program - // doesn't compile :-( + // program doesn't compile :-( h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data, size_t, size_t) { const std::string s = "

Hello world!

"; diff --git a/project_11_path_planning/src/utils.h b/project_11_path_planning/src/utils.h index 6897dd7..4fc69f1 100644 --- a/project_11_path_planning/src/utils.h +++ b/project_11_path_planning/src/utils.h @@ -7,6 +7,10 @@ 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) + // For converting back and forth between radians and degrees. double deg2rad(double x) { return x * pi / 180; } double rad2deg(double x) { return x * 180 / pi; }