/// <summary>
/// Static all-in-one method to fit the splines and evaluate at X coordinates.
/// </summary>
/// <param name="x">Input. X coordinates to fit.</param>
/// <param name="y">Input. Y coordinates to fit.</param>
/// <param name="xs">Input. X coordinates to evaluate the fitted curve at.</param>
/// <param name="startSlope">Optional slope constraint for the first point. Single.NaN means no constraint.</param>
/// <param name="endSlope">Optional slope constraint for the final point. Single.NaN means no constraint.</param>
/// <param name="debug">Turn on console output. Default is false.</param>
/// <returns>The computed y values for each xs.</returns>
public static double[] Compute(double[] x, double[] y, double[] xs, double startSlope = double.NaN, double endSlope = double.NaN, bool debug = false)
{
CubicSpline spline = new CubicSpline();
return(spline.FitAndEval(x, y, xs, startSlope, endSlope, debug));
}
private static void OnMessage(string message)
{
message = HasData(message);
if (message == null)
{
_socket.Send("42[\"manual\",{}]");
return;
}
Message msg = JsonConvert.DeserializeObject <Message>(message);
double brake_speed = 0;
bool car_in_front = false;
for (int i = 0; i < msg.sensor_fusion.Length; i++)
{
double[] car_data = msg.sensor_fusion[i];
double vx = car_data[3];
double vy = car_data[4];
double s = car_data[5];
double d = car_data[6];
if (d > lane * LANE_WIDTH && d < (lane + 1) * LANE_WIDTH)
{
double distance = s - msg.s;
double distance_threshold = msg.speed / 1.2;
if (s > msg.s && distance < distance_threshold)
{
double distance_violation = distance_threshold - distance;
double car_vel_mph = Math.Sqrt(vx * vx + vy * vy) * MS_TO_MPH;
double speed_diff = desired_velocity_mph - car_vel_mph;
brake_speed = speed_diff * BRAKE_SPEED_FACTOR + distance_violation * BRAKE_DIST_FACTOR;
car_in_front = true;
break;
}
}
}
if (car_in_front)
{
brake_speed = brake_speed > MAX_SPEED_DIFF ? MAX_SPEED_DIFF : brake_speed;
desired_velocity_mph -= brake_speed;
}
// Don't use msg.speed, it lags behind and will cause desired velocity to get too high
else if (desired_velocity_mph < TARGET_VELOCITY_MPH)
{
desired_velocity_mph += MAX_SPEED_DIFF;
}
if (current_action == null)
{
LaneChangeInfo info_left = CheckCarsOnAdjacentLane(-1, msg.s, msg.sensor_fusion);
LaneChangeInfo info_right = CheckCarsOnAdjacentLane(1, msg.s, msg.sensor_fusion);
List <PathPlannerAction> actions = new List <PathPlannerAction>();
double straight_cost = Math.Abs(TARGET_VELOCITY_MPH - desired_velocity_mph) * SPEED_COST + Math.Abs(lane - DESIRED_LANE) * LANE_COST;
actions.Add(new PathPlannerAction(0, straight_cost));
// TODO: Use timer
int time_since_last_last_change = Environment.TickCount - last_lane_change;
if (time_since_last_last_change > TIME_UNTIL_NEXT_LANE_CHANGE_MS && (lane != DESIRED_LANE || car_in_front))
{
if (info_left != null)
{
double lane_change_left_cost = Math.Abs((lane - 1) - DESIRED_LANE) * LANE_COST + info_left.AdjacentCarDeltaSpeed * SPEED_COST;
actions.Add(new PathPlannerAction(-1, lane_change_left_cost));
}
if (info_right != null)
{
double lane_change_right_cost = Math.Abs((lane + 1) - DESIRED_LANE) * LANE_COST + info_right.AdjacentCarDeltaSpeed * SPEED_COST;
actions.Add(new PathPlannerAction(1, lane_change_right_cost));
}
}
// Round the costs to filter out jitters. If values are the same, straight will be preferred
// as this action is the first one in the list
PathPlannerAction desired_action = actions.OrderBy(a => Math.Round(a.Cost)).First();
lane += desired_action.LaneShift;
if (desired_action.LaneShift != 0)
{
current_action = desired_action;
}
}
else
{
if (msg.d > lane * LANE_WIDTH && msg.d < (lane + 1) * LANE_WIDTH)
{
current_action = null;
last_lane_change = Environment.TickCount;
}
}
double desired_velocity_ms = desired_velocity_mph * MPH_TO_MS;
double smooth_step = desired_velocity_ms * TIME_TO_REACH_POINT;
double[] rough_x = new double[NUMBER_OF_ROUGH_POINTS];
double[] rough_y = new double[NUMBER_OF_ROUGH_POINTS];
int prev_size = msg.previous_path_x.Length;
double ref_x = msg.x;
double ref_y = msg.y;
double ref_yaw = deg2rad(msg.yaw);
if (prev_size < 2)
{
double prev_car_x = msg.x - Math.Cos(msg.yaw);
double prev_car_y = msg.y - Math.Sin(msg.yaw);
rough_x[0] = prev_car_x;
rough_x[1] = msg.x;
rough_y[0] = prev_car_y;
rough_y[1] = msg.y;
}
else
{
ref_x = msg.previous_path_x[prev_size - 1];
ref_y = msg.previous_path_y[prev_size - 1];
double ref_x_prev = msg.previous_path_x[prev_size - 2];
double ref_y_prev = msg.previous_path_y[prev_size - 2];
ref_yaw = Math.Atan2(ref_y - ref_y_prev, ref_x - ref_x_prev);
rough_x[0] = ref_x_prev;
rough_x[1] = ref_x;
rough_y[0] = ref_y_prev;
rough_y[1] = ref_y;
}
for (int i = 2; i < NUMBER_OF_ROUGH_POINTS; i++)
{
double next_s = msg.s + (i - 1) * ROUGH_STEP_SIZE * (current_action == null ? 1.0 : LANE_CHANGE_ROUGH_STEP_SIZE_FACTOR);
double next_d = GetCenterOfLane(lane);
double[] xy = getXY(next_s, next_d);
rough_x[i] = xy[0];
rough_y[i] = xy[1];
}
double yaw_sin = Math.Sin(-ref_yaw);
double yaw_cos = Math.Cos(-ref_yaw);
for (int i = 0; i < NUMBER_OF_ROUGH_POINTS; i++)
{
double shift_x = rough_x[i] - ref_x;
double shift_y = rough_y[i] - ref_y;
rough_x[i] = shift_x * yaw_cos - shift_y * yaw_sin;
rough_y[i] = shift_x * yaw_sin + shift_y * yaw_cos;
}
CubicSpline spline = new CubicSpline(rough_x, rough_y);
double[] next_x_vals = new double[NUMBER_OF_PATH_POINTS];
double[] next_y_vals = new double[NUMBER_OF_PATH_POINTS];
for (int i = 0; i < prev_size; i++)
{
next_x_vals[i] = msg.previous_path_x[i];
next_y_vals[i] = msg.previous_path_y[i];
}
yaw_sin = Math.Sin(ref_yaw);
yaw_cos = Math.Cos(ref_yaw);
for (int i = 0; i < next_x_vals.Length - prev_size; i++)
{
double x_point = (i + 1) * smooth_step;
double y_point = spline.Eval(new double[] { x_point })[0];
double x_ref = x_point;
x_point = x_ref * yaw_cos - y_point * yaw_sin;
y_point = x_ref * yaw_sin + y_point * yaw_cos;
x_point += ref_x;
y_point += ref_y;
next_x_vals[prev_size + i] = x_point;
next_y_vals[prev_size + i] = y_point;
}
JObject answer = new JObject();
answer.Add("next_x", JToken.FromObject(next_x_vals));
answer.Add("next_y", JToken.FromObject(next_y_vals));
string answer_str = "42[\"control\"," + answer.ToString() + "]";
_socket.Send(answer_str);
}
