beam_search_strategy.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2022 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#include "beam_search_strategy.hpp"
 
namespace arbitrator
{
    std::vector<std::pair<carma_planning_msgs::msg::ManeuverPlan, double>> BeamSearchStrategy::prioritize_plans(std::vector<std::pair<carma_planning_msgs::msg::ManeuverPlan, double>> plans) const
    {
        std::sort(plans.begin(), 
            plans.end(), 
            [] (const std::pair<carma_planning_msgs::msg::ManeuverPlan, double>& a, const std::pair<carma_planning_msgs::msg::ManeuverPlan, double>& b) 
            {
                return a.second < b.second;
            }
        );
 
        if (plans.size() > beam_width_)
        {
            plans.resize(beam_width_);
        }
        
        return plans;
    }
}