src/planning/driving_strategy.cpp
/**
     *  whether the trajectory is feasible
     * 
     *  @param trajectory 
     *  @param stepsize
     *  @return true if feasible false otherwise
    */
whether the trajectory is feasible

@param trajectory
@param stepsize
@return true if feasible false otherwise
whether the trajectory is feasible @param trajectory @param stepsize @return true if feasible false otherwise
bool Highway::is_feasible_trajectory(const TrajectoryGenerator::Trajectory &trajectory, double stepsize) {
        for (int i = 0; i < trajectory.N - 3; ++i) {
            double vx = get_velocity_forward_difference(i, trajectory.x, stepsize);
            double vy = get_velocity_forward_difference(i, trajectory.y, stepsize);
            double ax = get_acceleration_forward_difference(i, trajectory.x, stepsize);
            double ay = get_acceleration_forward_difference(i, trajectory.y, stepsize);
            double jx = get_jerk_forward_difference(i, trajectory.x, stepsize);
            double jy = get_jerk_forward_difference(i, trajectory.y, stepsize);
            double v = std::sqrt(vx*vx + vy*vy);
            double a = std::sqrt(ax*ax + ay*ay);
            double j = std::sqrt(jx*jx + jy*jy);
            if (!is_feasible_velocity(v)) {
                return false;
            }
        }
        return true;
    }
bool Highway :: is_feasible_trajectory ( const TrajectoryGenerator :: Trajectory & trajectory , double stepsize ) { for ( int i = 0 ; i < trajectory . N - 3 ; ++ i ) { double vx = get_velocity_forward_difference ( i , trajectory . x , stepsize ) ; double vy = get_velocity_forward_difference ( i , trajectory . y , stepsize ) ; double ax = get_acceleration_forward_difference ( i , trajectory . x , stepsize ) ; double ay = get_acceleration_forward_difference ( i , trajectory . y , stepsize ) ; double jx = get_jerk_forward_difference ( i , trajectory . x , stepsize ) ; double jy = get_jerk_forward_difference ( i , trajectory . y , stepsize ) ; double v = std :: sqrt ( vx * vx + vy * vy ) ; double a = std :: sqrt ( ax * ax + ay * ay ) ; double j = std :: sqrt ( jx * jx + jy * jy ) ; if ( ! is_feasible_velocity ( v ) ) { return false ; } } return true ; }
whether the trajectory is feasible
whether the trajectory is feasible
// violate dynamic constraints -- infeasible // std::cout << "[Feasibility Violation]: " << v << ", " << a << ", " << j << " @ " << i << "--" << trajectory.x[i] << ", " << trajectory.y[i] << std::endl;