Function frenet_optimal_planning 

pub fn frenet_optimal_planning(
    csp: &CubicSpline2D,
    s0: f64,
    c_speed: f64,
    c_d: f64,
    c_d_d: f64,
    c_d_dd: f64,
    ob: &[(f64, f64)],
) -> Option<FrenetPath>

Frenet optimal planning