Skip to content

Commit

Permalink
Refactor and split code into files
Browse files Browse the repository at this point in the history
  • Loading branch information
@ndrplz
ndrplz committed Aug 17, 2017
1 parent a24d792 commit 85704f5
Show file tree
Hide file tree
Showing 4 changed files with 140 additions and 210 deletions.
69 changes: 69 additions & 0 deletions project_11_path_planning/src/coords_transform.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,69 @@
#include "waypoints.h"


// Transform from Cartesian x,y coordinates to Frenet s,d coordinates
std::vector<double> cartesian_to_frenet(double x, double y, double theta, std::vector<double> maps_x, std::vector<double> 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<double> frenet_to_cartesian(double s, double d, std::vector<double> maps_s, std::vector<double> maps_x, std::vector<double> 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 };
}
220 changes: 10 additions & 210 deletions project_11_path_planning/src/main.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,25 +1,19 @@
#include <fstream>
#include <math.h>
#include <uWS/uWS.h>
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include <limits>
#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,
Expand All @@ -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<double> maps_x, vector<double> maps_y) {

double closest_len = std::numeric_limits<int>::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<double> maps_x, vector<double> 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<double> getFrenet(double x, double y, double theta, vector<double> maps_x, vector<double> 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<double> getXY(double s, double d, vector<double> maps_s, vector<double> maps_x, vector<double> 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;
Expand Down Expand Up @@ -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;
}
}
}

Expand Down Expand Up @@ -297,9 +177,9 @@ int main() {
}

// In Frenet coordinates, add evenly 30m spaced points ahead of the starting reference
vector<double> next_wp0 = getXY(car_s + 30, (2 + 4 * lane), map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> next_wp1 = getXY(car_s + 60, (2 + 4 * lane), map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> next_wp2 = getXY(car_s + 90, (2 + 4 * lane), map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> next_wp0 = frenet_to_cartesian(car_s + 30, (2 + 4 * lane), map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> next_wp1 = frenet_to_cartesian(car_s + 60, (2 + 4 * lane), map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> 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]);
Expand Down Expand Up @@ -406,83 +286,3 @@ int main() {
}
h.run();
}
















































































19 changes: 19 additions & 0 deletions project_11_path_planning/src/utils.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
#ifndef UTILS_H
#define UTILS_H

#include <math.h>
#include <limits>
#include <vector>

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
42 changes: 42 additions & 0 deletions project_11_path_planning/src/waypoints.h
View file
Original file line number Diff line number Diff line change
@@ -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<double> maps_x, std::vector<double> maps_y) {

double closest_len = std::numeric_limits<int>::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<double> maps_x, std::vector<double> 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;
}

0 comments on commit 85704f5

Please sign in to comment.