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

@param trajectory_params
@param stepsize
@return true if feasible false otherwise
whether the trajectory is feasible @param trajectory_params @param stepsize @return true if feasible false otherwise
bool Highway::is_feasible_trajectory(const TrajectoryGenerator::TrajectoryParams &trajectory_params, double stepsize) {
        for (double t = 0.0; t < trajectory_params.T; t += stepsize) {
            double vs = TrajectoryGenerator::evaluate_polynomial_derivative(t, trajectory_params.s, 1);
            double as = TrajectoryGenerator::evaluate_polynomial_derivative(t, trajectory_params.s, 2);
            double js = TrajectoryGenerator::evaluate_polynomial_derivative(t, trajectory_params.s, 3);
            double vd = TrajectoryGenerator::evaluate_polynomial_derivative(t, trajectory_params.d, 1);
            double ad = TrajectoryGenerator::evaluate_polynomial_derivative(t, trajectory_params.d, 2);
            double jd = TrajectoryGenerator::evaluate_polynomial_derivative(t, trajectory_params.d, 3);
            double v = std::sqrt(vs*vs + vd*vd);
            double a = std::sqrt(as*as + ad*ad);
            double j = std::sqrt(js*js + jd*jd);
            if (!is_feasible_velocity(v) || !is_feasible_acceleration(a)) {
                return false;
            }
        }
        return true;
    }
bool Highway :: is_feasible_trajectory ( const TrajectoryGenerator :: TrajectoryParams & trajectory_params , double stepsize ) { for ( double t = 0.0 ; t < trajectory_params . T ; t += stepsize ) { double vs = TrajectoryGenerator :: evaluate_polynomial_derivative ( t , trajectory_params . s , 1 ) ; double as = TrajectoryGenerator :: evaluate_polynomial_derivative ( t , trajectory_params . s , 2 ) ; double js = TrajectoryGenerator :: evaluate_polynomial_derivative ( t , trajectory_params . s , 3 ) ; double vd = TrajectoryGenerator :: evaluate_polynomial_derivative ( t , trajectory_params . d , 1 ) ; double ad = TrajectoryGenerator :: evaluate_polynomial_derivative ( t , trajectory_params . d , 2 ) ; double jd = TrajectoryGenerator :: evaluate_polynomial_derivative ( t , trajectory_params . d , 3 ) ; double v = std :: sqrt ( vs * vs + vd * vd ) ; double a = std :: sqrt ( as * as + ad * ad ) ; double j = std :: sqrt ( js * js + jd * jd ) ; if ( ! is_feasible_velocity ( v ) || ! is_feasible_acceleration ( a ) ) { return false ; } } return true ; }
whether the trajectory is feasible
whether the trajectory is feasible
// generate trajectory for actuator: // violate dynamic constraints -- infeasible /*
                std::cout << "[Feasibility Violation]: " << v << ", " << a << ", " << j << std::endl;
                */