Skip to content

Latest commit

 

History

History
487 lines (411 loc) · 14.8 KB

utils.org

File metadata and controls

487 lines (411 loc) · 14.8 KB

Utilities for Path Planning

This is the code to be seperated from the main.cpp to be more modular.

Utilities

This header file contains implementations.

The functions of interpolate_points are from https://github.com/jeremy-shannon/CarND-Path-Planning-Project

The functions of NextWaypoint, ClosestWaypoint, hasData are from Udacity.

  #ifndef UTILS
  #define UTILS
  /*
  utils.h

  Here are utilities for path planning.

   */

  #include <math.h>
  #include <uWS/uWS.h>
  #include <chrono>
  #include <iostream>
  #include <thread>
  #include <vector>
  #include <algorithm>
  #include "parameters.h"

  using namespace std;

  // 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,
  // else the empty string "" will be returned.
  string hasData(string s) {
    auto found_null = s.find("null");
    auto b1 = s.find_first_of("[");
    auto b2 = s.find_first_of("}");
    if (found_null != string::npos) {
      return "";
    } else if (b1 != string::npos && b2 != string::npos) {
      return s.substr(b1, b2 - b1 + 2);
    }
    return "";
  }

  double distance(double x1, double y1, double x2, double y2)
  {
    return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
  }

  <<logistic>>
  <<lane_judgment>>

  <<wrap_around>>

  int ClosestWaypoint(double x, double y, vector<double> maps_x, vector<double> maps_y)
  {
    double closestLen = 100000; //large number
    int closestWaypoint = 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 < closestLen)
      {
        closestLen = dist;
        closestWaypoint = i;
      }
    }
    return closestWaypoint;
  }

<<next-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, vector<double> maps_s)
  {
    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 = maps_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};
  }

<<interpolate_points_function>>

<<support-to-maneuver>>
  #endif

Wrap around frenet s in a circle

Treat the s coordinate value in Frenet in a circle, when it’s increased beyond the total length of the circle drive, MAX_S, then wrap it around.

double wrap_around(double s) {
  while (s < 0)
    {
      s += MAX_S;
    }
  while (MAX_S <= s)
    {
      s -= MAX_S;
    }
  assert(0 <= s);
  assert(s < MAX_S);
  return s;
}

lane_judgment

int lane_width = 4;
// starting from 0, from the left most to the right most
int lane_center_d(int lane_index) {
  return (lane_index + 0.5)*lane_width;
}

bool within_lane(int lane, double d) {
  return (lane*lane_width < d) && (d < (lane+1)*lane_width);
 }

int d_to_lane_index(double d) {
  // assert(0 <= d);
  // assert(d <= lane_width*NUM_LANES);
  int lane_index = (int)floor(d/lane_width);
  if (lane_index < 0) {
    cout << "Negative lane index: " << lane_index << ", d: " << d << endl;
  }
  if (NUM_LANES < lane_index) {
    cout << "lane index beyond NUM_LANES: " << NUM_LANES << ", lane_index: " << lane_index << ", d: " << d << endl;
  }
  //assert(0 <= lane_index);
  //assert(lane_index < NUM_LANES);
  return lane_index;
}

Utilities (old)

Compute the NextWaypoint

  • distance: compute the distance between two points, used in ClosestWaypoint
  • ClosestWaypoint: find the closest waypoint to the point (x, y) in terms of index to the arrays of {x, y}-coordinates of the waypoints
  • NextWaypoint: the next waypoint relative to the point (x, y) in terms of the index of waypoints, using ClosestWaypoint.
double distance(double x1, double y1, double x2, double y2) {
  return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
}

int ClosestWaypoint(double x, double y, vector<double> maps_x, vector<double> maps_y) {
  /* maps_x, and maps_y are the {x, y}-coordinates of the waypoints.
     Returns the index of the waypoint that is closest to the point (x, y)
   */
  double closestLen = 100000; //large number
  int closestWaypoint = 0;

  for(size_t 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 < closestLen) {
      closestLen = dist;
      closestWaypoint = i;
    }
  }
  return closestWaypoint;
}

int NextWaypoint(double x, double y, double theta, vector<double> maps_x, vector<double> maps_y) {
  /*
    maps_x, and maps_y are the {x, y}-coordinates of the waypoints.
    returns the next waypoint relative to the point (x, y) in terms of the index of waypoints.
    */
  int closestWaypoint = ClosestWaypoint(x, y, maps_x, maps_y);

  double map_x = maps_x[closestWaypoint];
  double map_y = maps_y[closestWaypoint];
  double heading = atan2( (map_y-y),(map_x-x) );
  double angle = abs(theta-heading);
  if(angle > pi()/4) {          // The closest waypoint has been passed by the point (x, y)
    closestWaypoint++;
  }
  return closestWaypoint;
}

Coordinate conversation

  • getFrenet: from Cartesian to Frenet by way of waypoints
  • getXY: from Frenet to Cartesian
// 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; // circular path
  }

  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]; // offset relative to previous waypoint
  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};
}

hasData

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
string hasData(string s) {
  auto found_null = s.find("null");
  auto b1 = s.find_first_of("[");
  auto b2 = s.find_first_of("}");
  if (found_null != string::npos) {
    return "";
  } else if (b1 != string::npos && b2 != string::npos) {
    return s.substr(b1, b2 - b1 + 2);
  }
  return "";
}

NextWaypoint (old, suggested to have bug)

int NextWaypoint(double x, double y, double theta, vector<double> maps_x, vector<double> maps_y)
{
  int closestWaypoint = ClosestWaypoint(x,y,maps_x,maps_y);
  double map_x = maps_x[closestWaypoint];
  double map_y = maps_y[closestWaypoint];
  double heading = atan2( (map_y-y),(map_x-x) );
  double angle = abs(theta-heading);
  if(angle > pi()/4)
  {
    closestWaypoint++;
  }
  return closestWaypoint;
}

NextWaypoint (new, correction from forum)

Based on https://discussions.udacity.com/t/2-bugs-in-code-provided-in-nextwaypoint/397364

//int NextWaypoint(double x, double y, double theta, const vector<double> &maps_x, const vector<double> &maps_y)
int NextWaypoint(double x, double y, double theta, vector<double> maps_x, vector<double> maps_y)
{
  int closestWaypoint = ClosestWaypoint(x,y,maps_x,maps_y);
  double map_x = maps_x[closestWaypoint];
  double map_y = maps_y[closestWaypoint];

  double heading = atan2( (map_y-y),(map_x-x) );

  double angle = abs(theta - heading);
  angle = min(2*pi() - angle, angle); // XXX bug fix

  if(angle > pi()/4) {
      closestWaypoint++;
      if (closestWaypoint == maps_x.size()) {
          closestWaypoint = 0; // XXX bug fix
        }
    }
  // XXX debug
  // cout << "corrected closestWaypoint=" << closestWaypoint << endl;
  return closestWaypoint;
}

logistic

Support function for cost functions

double logistic(double x) {
  // returns a value between 0 and 1 for x in the range[0, infinity] and
  // - 1 to 0 for x in the range[-infinity, infinity].
  // Useful for cost functions.
  return 2.0 / (1 + exp(-x)) - 1.0;
  }

interpolate_points_function

vector<double> interpolate_points(vector<double> pts_x, vector<double> pts_y,
                                  vector<double> eval_at_x) {
  // uses the spline library to interpolate points connecting a series of x and y values
  // output is spline evaluated at each eval_at_x point

  if (pts_x.size() != pts_y.size()) {
    cout << "ERROR! SMOOTHER: interpolate_points size mismatch between pts_x and pts_y" << endl;
    return { 0 };
  }

  tk::spline s;
  s.set_points(pts_x,pts_y);    // currently it is required that X is already sorted
  vector<double> output;
  for (double x: eval_at_x) {
    output.push_back(s(x));
  }
  return output;
}

vector<double> interpolate_points(vector<double> pts_x, vector<double> pts_y,
                                  double interval, int output_size) {
  // uses the spline library to interpolate points connecting a series of x and y values
  // output is output_size number of y values beginning at y[0] with specified fixed interval

  if (pts_x.size() != pts_y.size()) {
    cout << "ERROR! SMOOTHER: interpolate_points size mismatch between pts_x and pts_y" << endl;
    return { 0 };
  }

  tk::spline s;
  s.set_points(pts_x,pts_y);    // currently it is required that X is already sorted
  vector<double> output;
  for (int i = 0; i < output_size; i++) {
    output.push_back(s(pts_x[0] + i * interval));
  }
  return output;
}

support-to-maneuver

template <typename T>
void vector_remove(vector<T> & a_vector, T value) {
  a_vector.erase(std::remove(a_vector.begin(), a_vector.end(), value), a_vector.end());
}

template <typename T>
typename T::iterator min_map_element(T& m) {
  return min_element(m.begin(), m.end(),
                     [](typename T::value_type& l,
                        typename T::value_type& r) -> bool { return l.second.cost < r.second.cost; });
}

constexpr unsigned int str2int(const char* str, int h = 0)
{
  return !str[h] ? 5381 : (str2int(str, h+1) * 33) ^ str[h];
}