double2
polyroot(const double2 *c, int n, const double2 x0, int max_iter,
double thresh)
{
double2 x = x0;
double2 f;
double2 *d = DD_STATIC_CAST(double2 *, calloc(sizeof(double2), n));
int conv = 0;
int i;
double max_c = fabs(dd_to_double(c[0]));
double v;
if (thresh == 0.0) {
thresh = DD_C_EPS;
}
/* Compute the coefficients of the derivatives. */
for (i = 1; i <= n; i++) {
v = fabs(dd_to_double(c[i]));
if (v > max_c) {
max_c = v;
}
d[i - 1] = dd_mul_dd_d(c[i], DD_STATIC_CAST(double, i));
}
thresh *= max_c;
/* Newton iteration. */
for (i = 0; i < max_iter; i++) {
f = polyeval(c, n, x);
if (fabs(dd_to_double(f)) < thresh) {
conv = 1;
break;
}
x = dd_sub(x, (dd_div(f, polyeval(d, n - 1, x))));
}
free(d);
if (!conv) {
dd_error("(dd_polyroot): Failed to converge.");
return DD_C_NAN;
}
return x;
}
static int numchanges(int np, polynomial *sseq, DBL a)
{
int changes;
DBL f, lf;
polynomial *s;
changes = 0;
lf = polyeval(a, sseq[0].ord, sseq[0].coef);
for (s = sseq + 1; s <= sseq + np; s++)
{
f = polyeval(a, s->ord, s->coef);
if (lf == 0.0 || lf * f < 0)
{
changes++;
}
lf = f;
}
return(changes);
}
int bush(GF & gf, bclib::matrix<int> & A, int str, int ncol)
{
int q = gf.q;
std::vector<int> coef(str);
// bushcheck throws if it fails
bushcheck(q, str, ncol);
for (size_t i = 0; i < static_cast<size_t>(primes::ipow(q, str)); i++)
{
itopoly(static_cast<int>(i), q, str - 1, coef);
A(i, static_cast<size_t>(0)) = coef[static_cast<size_t>(str) - 1];
for (size_t j = 0; j < static_cast<size_t>(ncol) - 1; j++)
{
polyeval(gf, str - 1, coef, static_cast<int>(j), &(A(i, 1 + j)));
}
}
return SUCCESS_CHECK;
}
int main()
{
uWS::Hub h;
// MPC is initialized here!
MPC mpc;
h.onMessage([&mpc](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length,
uWS::OpCode opCode) {
// "42" at the start of the message means there's a websocket message event.
// The 4 signifies a websocket message
// The 2 signifies a websocket event
string sdata = string(data).substr(0, length);
cout << sdata << endl;
if (sdata.size() > 2 && sdata[0] == '4' && sdata[1] == '2')
{
string s = hasData(sdata);
if (s != "")
{
auto j = json::parse(s);
string event = j[0].get<string>();
if (event == "telemetry")
{
// j[1] is the data JSON object
vector<double> ptsx = j[1]["ptsx"];
vector<double> ptsy = j[1]["ptsy"];
double px = j[1]["x"];
double py = j[1]["y"];
double psi = j[1]["psi"];
double v = j[1]["speed"];
v *= 0.44704; //convert from mph to m/s
double steer_value_in = j[1]["steering_angle"];
steer_value_in *= deg2rad(25);
double throttle_value_in = j[1]["throttle"];
/*
* TODO: Calculate steering angle and throttle using MPC.
*
* Both are in between [-1, 1].
*
*/
Eigen::VectorXd ptsx_car = Eigen::VectorXd(ptsx.size());
Eigen::VectorXd ptsy_car = Eigen::VectorXd(ptsx.size());
// Convert from the map coordinate system to the vehicle coordinate system
for (int i = 0; i < ptsx.size(); i++)
{
auto car_coord = map_to_car_coord(psi, px, py, ptsx[i], ptsy[i]);
ptsx_car[i] = car_coord[0];
ptsy_car[i] = car_coord[1];
}
auto coeffs = polyfit(ptsx_car, ptsy_car, 3);
double cte = polyeval(coeffs, 0) - 0.0;
double epsi = -atan(coeffs[1]);
// Create current state vector and solve
// Add a latency of 100ms into the state before sending it to solver
Eigen::VectorXd state(6);
double latency = 0.1; //add a latency of 100ms
double Lf = 2.67;
double x_dl = (0.0 + v * latency);
double y_dl = 0.0;
double psi_dl = 0.0 + v * steer_value_in / Lf * latency;
double v_dl = 0.0 + v + throttle_value_in * latency;
double cte_dl = cte + (v * sin(epsi) * latency);
double epsi_dl = epsi + v * steer_value_in / Lf * latency;
state << x_dl, y_dl, psi_dl, v_dl, cte_dl, epsi_dl;
auto result = mpc.Solve(state, coeffs);
double steer_value = -result[6] / deg2rad(25);
double throttle_value = result[7];
json msgJson;
msgJson["steering_angle"] = steer_value;
msgJson["throttle"] = throttle_value;
//Display the MPC predicted trajectory
vector<double> mpc_x_vals = mpc.solution_x_;
vector<double> mpc_y_vals = mpc.solution_y_;
//.. add (x,y) points to list here, points are in reference to the vehicle's coordinate system
vector<double> next_x;
vector<double> next_y;
for (int i = 0; i < ptsx.size(); i++)
{
//auto car_coord = map_to_car_coord(psi, px, py, ptsx[i], ptsy[i]);
next_x.push_back(ptsx_car[i]);
next_y.push_back(ptsy_car[i]);
}
msgJson["mpc_x"] = mpc_x_vals;
msgJson["mpc_y"] = mpc_y_vals;
msgJson["next_x"] = next_x;
msgJson["next_y"] = next_y;
auto msg = "42[\"steer\"," + msgJson.dump() + "]";
std::cout << msg << std::endl;
// Latency
// The purpose is to mimic real driving conditions where
// the car does actuate the commands instantly.
//
// Feel free to play around with this value but should be to drive
// around the track with 100ms latency.
//
// NOTE: REMEMBER TO SET THIS TO 100 MILLISECONDS BEFORE
// SUBMITTING.
this_thread::sleep_for(chrono::milliseconds((int)(latency * 1000)));
ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
}
}
else
{
// Manual driving
std::string msg = "42[\"manual\",{}]";
ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
}
}
});
// We don't need this since we're not using HTTP but if it's removed the
// program
// doesn't compile :-(
h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data,
size_t, size_t) {
const std::string s = "<h1>Hello world!</h1>";
if (req.getUrl().valueLength == 1)
{
res->end(s.data(), s.length());
}
else
{
// i guess this should be done more gracefully?
res->end(nullptr, 0);
}
});
h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
std::cout << "Connected!!!" << std::endl;
});
h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
char *message, size_t length) {
ws.close();
std::cout << "Disconnected" << std::endl;
});
int port = 4567;
if (h.listen(port))
{
std::cout << "Listening to port " << port << std::endl;
}
else
{
std::cerr << "Failed to listen to port" << std::endl;
return -1;
}
h.run();
}
static int regula_falsa(int order, DBL *coef, DBL a, DBL b, DBL *val)
{
int its;
DBL fa, fb, x, fx, lfx;
fa = polyeval(a, order, coef);
fb = polyeval(b, order, coef);
if (fa * fb > 0.0)
{
return 0;
}
if (fabs(fa) < SMALL_ENOUGH)
{
*val = a;
return(1);
}
if (fabs(fb) < SMALL_ENOUGH)
{
*val = b;
return(1);
}
lfx = fa;
for (its = 0; its < MAX_ITERATIONS; its++)
{
x = (fb * a - fa * b) / (fb - fa);
fx = polyeval(x, order, coef);
if (fabs(x) > RELERROR)
{
if (fabs(fx / x) < RELERROR)
{
*val = x;
return(1);
}
}
else
{
if (fabs(fx) < RELERROR)
{
*val = x;
return(1);
}
}
if (fa < 0)
{
if (fx < 0)
{
a = x;
fa = fx;
if ((lfx * fx) > 0)
{
fb /= 2;
}
}
else
{
b = x;
fb = fx;
if ((lfx * fx) > 0)
{
fa /= 2;
}
}
}
else
{
if (fx < 0)
{
b = x;
fb = fx;
if ((lfx * fx) > 0)
{
fa /= 2;
}
}
else
{
a = x;
fa = fx;
if ((lfx * fx) > 0)
{
fb /= 2;
}
}
}
/* Check for underflow in the domain */
if (fabs(b-a) < RELERROR)
{
*val = x;
return(1);
}
lfx = fx;
}
return(0);
}