arbitrator::CostSystemCostFunction Class Reference

Implementation of the CostFunction interface. More...

#include <cost_system_cost_function.hpp>

Inheritance diagram for arbitrator::CostSystemCostFunction:

Collaboration diagram for arbitrator::CostSystemCostFunction:

Public Member Functions
	CostSystemCostFunction ()
	Constructor for FixedPriorityCostFunction. More...
void	init (std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh)
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
carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::ComputePlanCost >	cost_system_sc_
bool	initialized_ = false

Detailed Description

Implementation of the CostFunction interface.

Implements costs by utilizing a ROS service call to the CARMA cost plugin system. Passes on the input maneuver plan to the Cost Plugin System for computation of the actual cost and cost per unit distance.

Definition at line 36 of file cost_system_cost_function.hpp.

Constructor & Destructor Documentation

CostSystemCostFunction()

arbitrator::CostSystemCostFunction::CostSystemCostFunction ( )

inline

Constructor for FixedPriorityCostFunction.

Definition at line 42 of file cost_system_cost_function.hpp.

{};

Member Function Documentation

compute_cost_per_unit_distance()

double arbitrator::CostSystemCostFunction::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

Exceptions

std::logic_error

if not initialized

Implements arbitrator::CostFunction.

Definition at line 55 of file cost_system_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::CostSystemCostFunction::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

Exceptions

std::logic_error

if not initialized

Implements arbitrator::CostFunction.

Definition at line 31 of file cost_system_cost_function.cpp.

    {
        if (!initialized_) {
            throw std::logic_error("Attempt to use CostSystemCostFunction before initialization.");
        }
 
        double total_cost = std::numeric_limits<double>::infinity();
 
        auto service_message = std::make_shared<carma_planning_msgs::srv::ComputePlanCost::Request>();
        service_message->maneuver_plan = plan;
        
        auto resp = cost_system_sc_->async_send_request(service_message);
 
        auto future_status = resp.wait_for(std::chrono::milliseconds(500));
 
        if (future_status == std::future_status::ready){
            total_cost = resp.get()->plan_cost;
        } else {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "Unable to get cost for plan from CostPluginSystem due to service call failure.");
        }
 
        return total_cost;
    }

References cost_system_sc_, and initialized_.

Referenced by compute_cost_per_unit_distance().

Here is the caller graph for this function:

init()

void arbitrator::CostSystemCostFunction::init

(

std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >

nh

)

Initialize the CostSystemCostFunction to communicate over the network. Sets up any ROS service clients needed to interact with the cost plugin system.

Must be called before using this cost function implementation.

Parameters

nh	A publicly namespaced nodehandle

Definition at line 25 of file cost_system_cost_function.cpp.

    {
        cost_system_sc_ = nh->create_client<carma_planning_msgs::srv::ComputePlanCost>("compute_plan_cost");
        initialized_ = true;
    }

References cost_system_sc_, and initialized_.

Referenced by arbitrator::ArbitratorNode::handle_on_configure().

Here is the caller graph for this function:

Member Data Documentation

cost_system_sc_

carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::ComputePlanCost> arbitrator::CostSystemCostFunction::cost_system_sc_

private

Definition at line 71 of file cost_system_cost_function.hpp.

Referenced by compute_total_cost(), and init().

initialized_

bool arbitrator::CostSystemCostFunction::initialized_ = false

private

Definition at line 72 of file cost_system_cost_function.hpp.

Referenced by compute_total_cost(), and init().

---

The documentation for this class was generated from the following files:

• arbitrator/include/cost_system_cost_function.hpp
• arbitrator/src/cost_system_cost_function.cpp