From 85704f5e446a665c3a1ba0dc175e668ddd35355d Mon Sep 17 00:00:00 2001 From: ndrplz Date: Thu, 17 Aug 2017 16:59:23 +0200 Subject: [PATCH] Refactor and split code into files --- .../src/coords_transform.h | 69 ++++++ project_11_path_planning/src/main.cpp | 220 +----------------- project_11_path_planning/src/utils.h | 19 ++ project_11_path_planning/src/waypoints.h | 42 ++++ 4 files changed, 140 insertions(+), 210 deletions(-) create mode 100644 project_11_path_planning/src/coords_transform.h create mode 100644 project_11_path_planning/src/utils.h create mode 100644 project_11_path_planning/src/waypoints.h diff --git a/project_11_path_planning/src/coords_transform.h b/project_11_path_planning/src/coords_transform.h new file mode 100644 index 0000000..d0cdf13 --- /dev/null +++ b/project_11_path_planning/src/coords_transform.h @@ -0,0 +1,69 @@ +#include "waypoints.h" + + +// Transform from Cartesian x,y coordinates to Frenet s,d coordinates +std::vector cartesian_to_frenet(double x, double y, double theta, std::vector maps_x, std::vector maps_y) +{ + int next_wp = get_next_waypoint(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 +std::vector frenet_to_cartesian(double s, double d, std::vector maps_s, std::vector maps_x, std::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/project_11_path_planning/src/main.cpp b/project_11_path_planning/src/main.cpp index 17e2fe9..872426d 100644 --- a/project_11_path_planning/src/main.cpp +++ b/project_11_path_planning/src/main.cpp @@ -1,25 +1,19 @@ #include -#include #include #include #include #include -#include -#include #include "Eigen-3.3/Eigen/Core" #include "Eigen-3.3/Eigen/QR" #include "json.hpp" #include "spline.h" +#include "utils.h" +#include "coords_transform.h" -using namespace std; -// for convenience +using namespace std; using json = nlohmann::json; -// 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(); } // Checks if the SocketIO event has JSON data. // If there is data the JSON object in string format will be returned, @@ -36,123 +30,6 @@ string hasData(string s) { return ""; } -// Calculate the Euclidea Distance between two points -double distance(double x1, double y1, double x2, double y2) { - return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); -} - -// Choose in the map of highway waypoints the one closest to the car -int ClosestWaypoint(double x, double y, vector maps_x, vector maps_y) { - - double closest_len = std::numeric_limits::max(); - int closest_waypoint = 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 < closest_len) { - closest_len = dist; - closest_waypoint = i; - } - } - - return closest_waypoint; - -} - -// Choose in the map of highway waypoints the closest before the car (that is the next). -// The actual closest waypoint could be behind the car. -int NextWaypoint(double x, double y, double theta, vector maps_x, vector maps_y){ - - int closest_waypoint = ClosestWaypoint(x,y,maps_x,maps_y); - - double map_x = maps_x[closest_waypoint]; - double map_y = maps_y[closest_waypoint]; - - double heading = atan2( (map_y-y),(map_x-x) ); - - double angle = abs(theta-heading); - - if(angle > pi()/4) - { - closest_waypoint++; - } - - return closest_waypoint; - -} - -// Transform from Cartesian x,y coordinates to Frenet s,d coordinates -vector getFrenet(double x, double y, double theta, vector maps_x, 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, vector maps_s, vector maps_x, 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}; - -} int main() { uWS::Hub h; @@ -254,9 +131,12 @@ int main() { // 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) + 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 (lane > 0) + lane = 0; + } } } @@ -297,9 +177,9 @@ int main() { } // In Frenet coordinates, add evenly 30m spaced 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); + 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); 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]); @@ -406,83 +286,3 @@ int main() { } h.run(); } - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/project_11_path_planning/src/utils.h b/project_11_path_planning/src/utils.h new file mode 100644 index 0000000..6897dd7 --- /dev/null +++ b/project_11_path_planning/src/utils.h @@ -0,0 +1,19 @@ +#ifndef UTILS_H +#define UTILS_H + +#include +#include +#include + +const double pi = M_PI; + +// 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; } + +// Calculate the Euclidea Distance between two points +double distance(double x1, double y1, double x2, double y2) { + return sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1)); +} + +#endif \ No newline at end of file diff --git a/project_11_path_planning/src/waypoints.h b/project_11_path_planning/src/waypoints.h new file mode 100644 index 0000000..56a812b --- /dev/null +++ b/project_11_path_planning/src/waypoints.h @@ -0,0 +1,42 @@ +#include "utils.h" + + +// Choose in the map of highway waypoints the one closest to the car +int get_closest_waypoint(double x, double y, std::vector maps_x, std::vector maps_y) { + + double closest_len = std::numeric_limits::max(); + int closest_waypoint = 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 < closest_len) { + closest_len = dist; + closest_waypoint = i; + } + } + return closest_waypoint; +} + + +// Choose in the map of highway waypoints the closest before the car (that is the next). +// The actual closest waypoint could be behind the car. +int get_next_waypoint(double x, double y, double theta, std::vector maps_x, std::vector maps_y) { + + int closest_waypoint = get_closest_waypoint(x, y, maps_x, maps_y); + + double map_x = maps_x[closest_waypoint]; + double map_y = maps_y[closest_waypoint]; + + double heading = atan2(map_y - y, map_x - x); + + double angle = abs(theta - heading); + + if (angle > pi / 4) + closest_waypoint++; + + return closest_waypoint; +} \ No newline at end of file