Implementation of the CostFunction interface. More...

#include <fixed_priority_cost_function.hpp>

Inheritance diagram for arbitrator::FixedPriorityCostFunction:

Collaboration diagram for arbitrator::FixedPriorityCostFunction:

Public Member Functions
	FixedPriorityCostFunction (const std::map< std::string, double > &plugin_priorities)
	Constructor for FixedPriorityCostFunction. More...

double	compute_total_cost (const carma_planning_msgs::msg::ManeuverPlan &plan)
	Compute the unit cost over distance of a given maneuver plan. More...

double	compute_cost_per_unit_distance (const carma_planning_msgs::msg::ManeuverPlan &plan)
	Compute the unit cost over distance of a given maneuver plan. More...

Public Member Functions inherited from arbitrator::CostFunction
virtual double	compute_total_cost (const carma_planning_msgs::msg::ManeuverPlan &plan)=0
	Compute the cost of a given maneuver plan. More...

virtual double	compute_cost_per_unit_distance (const carma_planning_msgs::msg::ManeuverPlan &plan)=0
	Compute the unit cost over distance of a given maneuver plan. More...

virtual	~CostFunction ()
	Virtual destructor provided for memory safety. More...

Private Attributes
std::map< std::string, double >	plugin_costs_

Detailed Description

Implementation of the CostFunction interface.

Implements costs by associating a fixed priority number with each plugin (as specified by configuration). This priority is then normalized across all plugins, and then an inverse is computed to arrive at the cost per unit distance for that plugins.

e.g. Three plugins with priority 20, 10, and 5 will respectively have costs 0, 0.5, 0.75 per unit distance.

Definition at line 37 of file fixed_priority_cost_function.hpp.

Constructor & Destructor Documentation

◆ FixedPriorityCostFunction()

arbitrator::FixedPriorityCostFunction::FixedPriorityCostFunction ( const std::map< std::string, double > & plugin_priorities )

Constructor for FixedPriorityCostFunction.

Parameters

plugin_priorities with plugin_name, priority_value mappings

Definition at line 25 of file fixed_priority_cost_function.cpp.

    {
        // Identify the highest priority values present in the list
        double max_priority = std::numeric_limits<double>::lowest();
        for (auto it = plugin_priorities.begin(); it != plugin_priorities.end(); it++)
        {
            if (it->second > max_priority)
            {
                max_priority = it->second;
            }
        }
 
        // Normalize the list and invert into costs
        for (auto it = plugin_priorities.begin(); it != plugin_priorities.end(); it++)
        {
            plugin_costs_[it->first] = 1.0 - (it->second / max_priority);
            RCLCPP_INFO_STREAM(rclcpp::get_logger("arbitrator"), "FixedPriorityCostFunction: " << "Plugin: " << it->first << " Normalized Cost: " << plugin_costs_[it->first]);
        }
    }

References plugin_costs_.

Member Function Documentation

◆ compute_cost_per_unit_distance()

double arbitrator::FixedPriorityCostFunction::compute_cost_per_unit_distance ( const carma_planning_msgs::msg::ManeuverPlan & plan )

virtual

Compute the unit cost over distance of a given maneuver plan.

Parameters

plan	The plan to evaluate

Returns: double The total cost divided by the total distance of the plan

Implements arbitrator::CostFunction.

Definition at line 58 of file fixed_priority_cost_function.cpp.

    {
        double plan_dist = arbitrator_utils::get_plan_end_distance(plan) - arbitrator_utils::get_plan_start_distance(plan);
        return compute_total_cost(plan) / plan_dist;
    }

References compute_total_cost(), arbitrator_utils::get_plan_end_distance(), and arbitrator_utils::get_plan_start_distance().

Here is the call graph for this function:

◆ compute_total_cost()

double arbitrator::FixedPriorityCostFunction::compute_total_cost ( const carma_planning_msgs::msg::ManeuverPlan & plan )

virtual

Compute the unit cost over distance of a given maneuver plan.

Parameters

plan	The plan to evaluate

Returns: double The total cost divided by the total distance of the plan

Implements arbitrator::CostFunction.

Definition at line 45 of file fixed_priority_cost_function.cpp.

    {
        double total_cost = 0.0;
        for (auto it = plan.maneuvers.begin(); it != plan.maneuvers.end(); it++)
        {
            std::string planning_plugin = GET_MANEUVER_PROPERTY(*it, parameters).planning_strategic_plugin;
            total_cost += (arbitrator_utils::get_maneuver_end_distance(*it) - arbitrator_utils::get_maneuver_start_distance(*it)) *
                plugin_costs_.at(planning_plugin);
        }
 
        return total_cost;
    }