src/planning/planner.cpp
/**
 * get lane feasible zone in surrounding environment
 * 
 * @param map: HD map
 * @param ego_vehicle: Ego vehicle
 * @param predicted_object_list: Predicted objects
 * @param horizon Planning horizon
 * @return Planned trajectory
 */
get lane feasible zone in surrounding environment


get lane feasible zone in surrounding environment
std::map<int, DrivingStrategy::Highway::LaneFeasibleZone> Planner::get_lane_feasible_zones(
    Map &map,
    EgoVehicle &ego_vehicle,
    const Predictor::PredictedObjectList &predicted_object_list,
    double horizon
) {
    std::map<int, DrivingStrategy::Highway::LaneFeasibleZone> lane_feasible_zones;
    const Map::Lane &ego_lane = map.localize_lane(ego_vehicle.s, ego_vehicle.d);
    TrajectoryGenerator::TrajectoryReference ego_trajectory_reference = ego_vehicle.get_trajectory_reference();
    double effective_acceleration = std::min(
        +0.9 * DrivingStrategy::MAX_ACCELERATION,
        ego_trajectory_reference.as + 0.5 * (0.9 * DrivingStrategy::MAX_JERK * horizon)
    );
    double effective_deceleration = std::max(
        -0.9 * DrivingStrategy::MAX_ACCELERATION,
        ego_trajectory_reference.as - 0.5 * (0.9 * DrivingStrategy::MAX_JERK * horizon)
    );
    auto feasible_maneuvers = map.get_valid_maneuvers(ego_lane);
    if (feasible_maneuvers.size() > 0) {
        lane_feasible_zones.clear();
        for (auto &feasible_maneuver: feasible_maneuvers) {
            const Map::Lane &next_lane = map.get_next_lane(ego_lane, feasible_maneuver);
            DrivingStrategy::Highway::LaneFeasibleZone lane_feasible_zone;
            lane_feasible_zone.is_with_leading_vehicle = false;
            lane_feasible_zone.is_with_following_vehicle = false;
            lane_feasible_zone.s_following_max = std::numeric_limits<double>::min();
            lane_feasible_zone.s_leading_min = std::numeric_limits<double>::max();
            lane_feasible_zone.s_lower = ego_vehicle.s + horizon * ego_trajectory_reference.vs + std::max(
                0.5 * std::pow(horizon, 2.0) * effective_deceleration,
                0.5 * std::pow(ego_trajectory_reference.vs, 2.0) / effective_deceleration
            );
            lane_feasible_zone.s_upper =  ego_vehicle.s + std::min(
                horizon * ego_trajectory_reference.vs + 0.5 * std::pow(horizon, 2.0) * effective_acceleration,
                horizon * next_lane.speed_limit_upper
            );
            lane_feasible_zone.vs_lower = std::max(
                0.0,
                ego_trajectory_reference.vs + horizon * effective_deceleration
            );
            lane_feasible_zone.vs_upper = std::min(
                next_lane.speed_limit_upper,
                ego_trajectory_reference.vs + horizon * effective_acceleration
            );
            lane_feasible_zones[next_lane.id] = lane_feasible_zone;
        }
        for (const auto &predicted_object: predicted_object_list) {
            size_t N = predicted_object.prediction.size();
            const Predictor::Prediction &init_pose = predicted_object.prediction[0];
            const Predictor::Prediction &terminal_pose = predicted_object.prediction[N - 1];
            double s0 = init_pose.s;
            double d0 = init_pose.d;
            double vs0 = init_pose.vs;
            double s1 = terminal_pose.s;
            double d1 = terminal_pose.d;
            double vs1 = terminal_pose.vs;
            const Map::Lane &object_lane = map.localize_lane(s0, d0);
            if (lane_feasible_zones.end() == lane_feasible_zones.find(object_lane.id)) {
                continue;
            }
            DrivingStrategy::Highway::LaneFeasibleZone &lane_feasible_zone = lane_feasible_zones[object_lane.id];
            if (s0 < ego_vehicle.s) {
                lane_feasible_zone.s_following_max = std::max(lane_feasible_zone.s_following_max, s1);
                if (object_lane.id != ego_lane.id) {
                    if (vs1 > ego_trajectory_reference.vs) {
                        double safety_distance = DrivingStrategy::Highway::get_safety_distance(vs1 - lane_feasible_zone.vs_lower);
                        double s_lower_proposed = s1 + safety_distance;
                        if (s_lower_proposed > lane_feasible_zone.s_lower) {
                            lane_feasible_zone.is_with_following_vehicle = true;
                            lane_feasible_zone.s_lower = s_lower_proposed;
                            lane_feasible_zone.vs_lower = vs1;
                        }
                    } else {
                    }
                } 
            } else {
                lane_feasible_zone.s_leading_min = std::min(lane_feasible_zone.s_leading_min, s1);
                if (vs1 < lane_feasible_zone.vs_upper) {
                    double safety_distance = DrivingStrategy::Highway::get_safety_distance(lane_feasible_zone.vs_upper - vs1);
                    double s_upper_proposed = s1 - safety_distance;
                    if (s_upper_proposed < lane_feasible_zone.s_upper) {
                        lane_feasible_zone.is_with_leading_vehicle = true;
                        lane_feasible_zone.s_upper = s_upper_proposed;
                        lane_feasible_zone.vs_upper = vs1;
                    }
                } else {
                }
            }
        }
        for (std::map<int, DrivingStrategy::Highway::LaneFeasibleZone>::iterator it = lane_feasible_zones.begin(); lane_feasible_zones.end() != it; ++it) {
            Map::LANE_ID intended_lane_id = (Map::LANE_ID) it->first;
            DrivingStrategy::Highway::LaneFeasibleZone &lane_feasible_zone = it->second;
            if (lane_feasible_zone.s_lower > lane_feasible_zone.s_upper || lane_feasible_zone.vs_lower > lane_feasible_zone.vs_upper) {
                lane_feasible_zones.erase(it);
                continue;
            }
            if (!lane_feasible_zone.is_with_leading_vehicle) {
                lane_feasible_zone.vs_lower = ego_trajectory_reference.vs;
                lane_feasible_zone.vs_upper = DrivingStrategy::MAX_VELOCITY;
            }
            if (intended_lane_id != ego_lane.id) {
                if (lane_feasible_zone.s_following_max > ego_trajectory_reference.s - DrivingStrategy::VEHICLE_LENGTH) {
                    lane_feasible_zones.erase(it);
                    continue;
                }
            }
        }
    }
    return lane_feasible_zones;
}
std :: map < int , DrivingStrategy :: Highway :: LaneFeasibleZone > Planner :: get_lane_feasible_zones ( Map & map , EgoVehicle & ego_vehicle , const Predictor :: PredictedObjectList & predicted_object_list , double horizon ) { std :: map < int , DrivingStrategy :: Highway :: LaneFeasibleZone > lane_feasible_zones ; const Map :: Lane & ego_lane = map . localize_lane ( ego_vehicle . s , ego_vehicle . d ) ; TrajectoryGenerator :: TrajectoryReference ego_trajectory_reference = ego_vehicle . get_trajectory_reference ( ) ; double effective_acceleration = std :: min ( +0.9 * DrivingStrategy :: MAX_ACCELERATION , ego_trajectory_reference . as + 0.5 * ( 0.9 * DrivingStrategy :: MAX_JERK * horizon ) ) ; double effective_deceleration = std :: max ( -0.9 * DrivingStrategy :: MAX_ACCELERATION , ego_trajectory_reference . as - 0.5 * ( 0.9 * DrivingStrategy :: MAX_JERK * horizon ) ) ; auto feasible_maneuvers = map . get_valid_maneuvers ( ego_lane ) ; if ( feasible_maneuvers . size ( ) > 0 ) { lane_feasible_zones . clear ( ) ; for ( auto & feasible_maneuver : feasible_maneuvers ) { const Map :: Lane & next_lane = map . get_next_lane ( ego_lane , feasible_maneuver ) ; DrivingStrategy :: Highway :: LaneFeasibleZone lane_feasible_zone ; lane_feasible_zone . is_with_leading_vehicle = false ; lane_feasible_zone . is_with_following_vehicle = false ; lane_feasible_zone . s_following_max = std :: numeric_limits < double > :: min ( ) ; lane_feasible_zone . s_leading_min = std :: numeric_limits < double > :: max ( ) ; lane_feasible_zone . s_lower = ego_vehicle . s + horizon * ego_trajectory_reference . vs + std :: max ( 0.5 * std :: pow ( horizon , 2.0 ) * effective_deceleration , 0.5 * std :: pow ( ego_trajectory_reference . vs , 2.0 ) / effective_deceleration ) ; lane_feasible_zone . s_upper = ego_vehicle . s + std :: min ( horizon * ego_trajectory_reference . vs + 0.5 * std :: pow ( horizon , 2.0 ) * effective_acceleration , horizon * next_lane . speed_limit_upper ) ; lane_feasible_zone . vs_lower = std :: max ( 0.0 , ego_trajectory_reference . vs + horizon * effective_deceleration ) ; lane_feasible_zone . vs_upper = std :: min ( next_lane . speed_limit_upper , ego_trajectory_reference . vs + horizon * effective_acceleration ) ; lane_feasible_zones [ next_lane . id ] = lane_feasible_zone ; } for ( const auto & predicted_object : predicted_object_list ) { size_t N = predicted_object . prediction . size ( ) ; const Predictor :: Prediction & init_pose = predicted_object . prediction [ 0 ] ; const Predictor :: Prediction & terminal_pose = predicted_object . prediction [ N - 1 ] ; double s0 = init_pose . s ; double d0 = init_pose . d ; double vs0 = init_pose . vs ; double s1 = terminal_pose . s ; double d1 = terminal_pose . d ; double vs1 = terminal_pose . vs ; const Map :: Lane & object_lane = map . localize_lane ( s0 , d0 ) ; if ( lane_feasible_zones . end ( ) == lane_feasible_zones . find ( object_lane . id ) ) { continue ; } DrivingStrategy :: Highway :: LaneFeasibleZone & lane_feasible_zone = lane_feasible_zones [ object_lane . id ] ; if ( s0 < ego_vehicle . s ) { lane_feasible_zone . s_following_max = std :: max ( lane_feasible_zone . s_following_max , s1 ) ; if ( object_lane . id != ego_lane . id ) { if ( vs1 > ego_trajectory_reference . vs ) { double safety_distance = DrivingStrategy :: Highway :: get_safety_distance ( vs1 - lane_feasible_zone . vs_lower ) ; double s_lower_proposed = s1 + safety_distance ; if ( s_lower_proposed > lane_feasible_zone . s_lower ) { lane_feasible_zone . is_with_following_vehicle = true ; lane_feasible_zone . s_lower = s_lower_proposed ; lane_feasible_zone . vs_lower = vs1 ; } } else { } } } else { lane_feasible_zone . s_leading_min = std :: min ( lane_feasible_zone . s_leading_min , s1 ) ; if ( vs1 < lane_feasible_zone . vs_upper ) { double safety_distance = DrivingStrategy :: Highway :: get_safety_distance ( lane_feasible_zone . vs_upper - vs1 ) ; double s_upper_proposed = s1 - safety_distance ; if ( s_upper_proposed < lane_feasible_zone . s_upper ) { lane_feasible_zone . is_with_leading_vehicle = true ; lane_feasible_zone . s_upper = s_upper_proposed ; lane_feasible_zone . vs_upper = vs1 ; } } else { } } } for ( std :: map < int , DrivingStrategy :: Highway :: LaneFeasibleZone > :: iterator it = lane_feasible_zones . begin ( ) ; lane_feasible_zones . end ( ) != it ; ++ it ) { Map :: LANE_ID intended_lane_id = ( Map :: LANE_ID ) it -> first ; DrivingStrategy :: Highway :: LaneFeasibleZone & lane_feasible_zone = it -> second ; if ( lane_feasible_zone . s_lower > lane_feasible_zone . s_upper || lane_feasible_zone . vs_lower > lane_feasible_zone . vs_upper ) { lane_feasible_zones . erase ( it ) ; continue ; } if ( ! lane_feasible_zone . is_with_leading_vehicle ) { lane_feasible_zone . vs_lower = ego_trajectory_reference . vs ; lane_feasible_zone . vs_upper = DrivingStrategy :: MAX_VELOCITY ; } if ( intended_lane_id != ego_lane . id ) { if ( lane_feasible_zone . s_following_max > ego_trajectory_reference . s - DrivingStrategy :: VEHICLE_LENGTH ) { lane_feasible_zones . erase ( it ) ; continue ; } } } } return lane_feasible_zones ; }
get lane feasible zone in surrounding environment
get lane feasible zone in surrounding environment
// localize ego vehicle: // current dynamics in frenet: // get valid maneuvers: // estimate feasible zone for each maneuver: // init containers for s lower & upper bounds: // next lane: // init as without leading vehicle: // init position region: // speed up at max acceleration // constant speed at lane upper speed // init velocity region:  /* DEBUG Feasible Zone
        std::cout << "[Init]: " << std::endl;
        for (std::map<int, DrivingStrategy::Highway::LaneFeasibleZone>::iterator it = lane_feasible_zones.begin(); lane_feasible_zones.end() != it; ++it) {
            Map::LANE_ID intended_lane_id = (Map::LANE_ID) it->first;
            DrivingStrategy::Highway::LaneFeasibleZone &lane_feasible_zone = it->second;

            std::cout << "\t[Lane]: " << it->first << std::endl;
            std::cout << "\t\t[Other Vehicle Presence]: " << lane_feasible_zone.is_with_leading_vehicle << ", " << lane_feasible_zone.is_with_following_vehicle << std::endl;
            std::cout << "\t\t[Available Area]: " << lane_feasible_zone.s_leading_min << ", " << lane_feasible_zone.s_following_max << std::endl;
            std::cout << "\t\t[Planning S]: " << lane_feasible_zone.s_upper << ", " << lane_feasible_zone.s_lower << std::endl;
            std::cout << "\t\t[Planning VS]: " << lane_feasible_zone.vs_upper << ", " << lane_feasible_zone.vs_lower << std::endl;
        }
        */ // set s lower & upper bounds according to surrounding object prediction: // init state: // terminal state: // occupied lane: // localize and classify surrounding objects according to their s coord relative to ego: // object behind ego: // too slow: // object in front of ego: // too fast: // eliminate infeasible zones: // std::cout << "[Final]: " << std::endl; /* DEBUG Feasible Zone
            std::cout << "\t[Lane]: " << it->first << std::endl;
            std::cout << "\t\t[Other Vehicle Presence]: " << lane_feasible_zone.is_with_leading_vehicle << ", " << lane_feasible_zone.is_with_following_vehicle << std::endl;
            std::cout << "\t\t[Available Area]: " << lane_feasible_zone.s_leading_min << ", " << lane_feasible_zone.s_following_max << std::endl;
            std::cout << "\t\t[Planning S]: " << lane_feasible_zone.s_upper << ", " << lane_feasible_zone.s_lower << std::endl;
            std::cout << "\t\t[Planning VS]: " << lane_feasible_zone.vs_upper << ", " << lane_feasible_zone.vs_lower << std::endl;
            */