Source code for commonroad_dc.costs.evaluation

import itertools
from enum import Enum
from typing import Dict, List, Tuple

from commonroad.planning.planning_problem import PlanningProblem, PlanningProblemSet
from commonroad.scenario.scenario import Scenario
from commonroad.scenario.trajectory import Trajectory
from commonroad_dc.costs.route_matcher import LaneletRouteMatcher, SolutionProperties, merge_trajectories

from commonroad.common.solution import Solution, CostFunction, VehicleType

import commonroad_dc.costs.partial_cost_functions as cost_functions


[docs]class PartialCostFunction(Enum): """ See https://gitlab.lrz.de/tum-cps/commonroad-cost-functions/-/blob/master/costFunctions_commonRoad.pdf for more details. A: Acceleration, J: Jerk, Jlat: Lateral Jerk, Jlon: Longitudinal Jerk, SA: Steering Angle, SR: Steering Rate, Y: Yaw Rate, LC: Lane Center Offset, V: Velocity Offset, Vlon: Longitudinal Velocity Offset, O: Orientation Offset, D: Distance to Obstacles, L: Path Length, T: Time, ID: Inverse Duration, """ A = "A" J = "J" Jlat = "Jlat" Jlon = "Jlon" SA = "SA" SR = "SR" Y = "Y" LC = "LC" V = "V" Vlon = "Vlon" O = "O" D = "D" L = "L" T = "T" ID = "ID"
PartialCostFunctionMapping = { PartialCostFunction.A: cost_functions.acceleration_cost, PartialCostFunction.J: cost_functions.jerk_cost, PartialCostFunction.Jlat: cost_functions.jerk_lat_cost, PartialCostFunction.Jlon: cost_functions.jerk_lon_cost, PartialCostFunction.SA: cost_functions.steering_angle_cost, PartialCostFunction.SR: cost_functions.steering_rate_cost, PartialCostFunction.Y: cost_functions.yaw_cost, PartialCostFunction.LC: cost_functions.lane_center_offset_cost, PartialCostFunction.V: cost_functions.velocity_offset_cost, PartialCostFunction.Vlon: cost_functions.longitudinal_velocity_offset_cost, PartialCostFunction.O: cost_functions.orientation_offset_cost, PartialCostFunction.D: cost_functions.distance_to_obstacle_cost, PartialCostFunction.L: cost_functions.lane_center_offset_cost, PartialCostFunction.T: cost_functions.time_cost, PartialCostFunction.ID: cost_functions.inverse_duration_cost, } cost_function_mapping =\ { CostFunction.JB1: [ (PartialCostFunction.T, 1.0) ], CostFunction.MW1: [ (PartialCostFunction.Jlat, 5.0), (PartialCostFunction.Jlon, 0.5), (PartialCostFunction.Vlon, 0.2), (PartialCostFunction.ID, 1.0) ], CostFunction.SA1: [ (PartialCostFunction.SA, 0.1), (PartialCostFunction.SR, 0.1), (PartialCostFunction.D, 100000.0), ], CostFunction.SM1: [ (PartialCostFunction.A, 50.0), (PartialCostFunction.SA, 50.0), (PartialCostFunction.SR, 50.0), (PartialCostFunction.L, 1.0), (PartialCostFunction.V, 20.0), (PartialCostFunction.O, 50.0), ], CostFunction.SM2: [ (PartialCostFunction.A, 50.0), (PartialCostFunction.SA, 50.0), (PartialCostFunction.SR, 50.0), (PartialCostFunction.L, 1.0), (PartialCostFunction.O, 50.0), ], CostFunction.SM3: [ (PartialCostFunction.A, 50.0), (PartialCostFunction.SA, 50.0), (PartialCostFunction.SR, 50.0), (PartialCostFunction.V, 20.0), (PartialCostFunction.O, 50.0), ], CostFunction.WX1: [ (PartialCostFunction.T, 10.0), (PartialCostFunction.V, 1.0), (PartialCostFunction.A, 0.1), (PartialCostFunction.J, 0.1), (PartialCostFunction.D, 0.1), (PartialCostFunction.L, 10.0), ], CostFunction.TR1: [ (PartialCostFunction.Jlon, 0.01), (PartialCostFunction.SR, 22), (PartialCostFunction.D, 8), (PartialCostFunction.LC, 0.5), ] } # additional attributes that need to be computed before evaluation required_properties = { PartialCostFunction.A: [], PartialCostFunction.J: [], PartialCostFunction.Jlat: [SolutionProperties.LatJerk], PartialCostFunction.Jlon: [SolutionProperties.LonJerk], PartialCostFunction.SA: [], PartialCostFunction.SR: [], PartialCostFunction.Y: [], PartialCostFunction.LC: [], PartialCostFunction.V: [], PartialCostFunction.Vlon: [SolutionProperties.LonVelocity, SolutionProperties.AllowedVelocityInterval], PartialCostFunction.O: [], PartialCostFunction.D: [SolutionProperties.LonDistanceObstacles], PartialCostFunction.L: [], PartialCostFunction.T: [], PartialCostFunction.ID: [], }
[docs]class CostFunctionEvaluator: def __init__(self, cost_function_id: CostFunction, vehicle_type: VehicleType): self.cost_function_id: CostFunction = cost_function_id self.vehicle_type = vehicle_type self.partial_cost_funcs: List[Tuple[PartialCostFunction,float]] = cost_function_mapping[self.cost_function_id]
[docs] @classmethod def init_from_solution(cls, solution: Solution): return cls(cost_function_id=CostFunction[solution.cost_ids[0]], vehicle_type=VehicleType(int(solution.vehicle_ids[0][-1])))
@property def required_properties(self): return list(itertools.chain.from_iterable(required_properties[p] for p, _ in self.partial_cost_funcs))
[docs] def evaluate_pp_solution(self, cr_scenario: Scenario, cr_pproblem: PlanningProblem, trajectory: Trajectory, draw_lanelet_path=False, debug_plot=False): """ Computes costs of one solution for cr_pproblem. :param cr_scenario: scenario :param cr_pproblem: planning problem that is solved by trajectory :param trajectory: solution trajectory :param draw_lanelet_path: optionally visualize the detected lanelet path with respect to whose some parameters for the cost computation are determined (only useful for development). :param debug_plot: show plot in case a trajectory cannot be transformed to curvilinear coordinates. :return: result of evaluation """ evaluation_result = PlanningProblemCostResult(cost_function_id=self.cost_function_id, solution_id=cr_pproblem.planning_problem_id) lm = LaneletRouteMatcher(cr_scenario, self.vehicle_type) trajectory, _, properties = lm.compute_curvilinear_coordinates(trajectory, required_properties=self.required_properties, draw_lanelet_path=draw_lanelet_path, debug_plot=debug_plot) for pcf, weight in self.partial_cost_funcs: pcf_func = PartialCostFunctionMapping[pcf] evaluation_result.add_partial_costs(pcf, pcf_func(cr_scenario, cr_pproblem, trajectory, properties), weight) return evaluation_result
[docs] def evaluate_solution(self, scenario: Scenario, cr_pproblems: PlanningProblemSet, solution: Solution)\ -> "SolutionResult": """ Computes costs for all solutions of a planning problem set. :param scenario: scenario that was solved :param cr_pproblems: planning problem set that was solved :param solution: Solution object that contains trajectories :return: SolutionResult object that contains partial and total costs """ results = SolutionResult(benchmark_id=solution.benchmark_id) for pps in solution.planning_problem_solutions: results.add_results( self.evaluate_pp_solution(scenario, cr_pproblems.planning_problem_dict[pps.planning_problem_id], pps.trajectory, False)) return results
[docs]class PlanningProblemCostResult: def __init__(self, cost_function_id: CostFunction, solution_id: int): """ Contains results of a single solution of a planning problem. """ self.cost_function_id = cost_function_id self.partial_costs: Dict[PartialCostFunction, float] = {} self.weights: Dict[PartialCostFunction, float] = {} self.solution_id: int = solution_id @property def total_costs(self) -> float: c = 0.0 for pcf, cost in self.partial_costs.items(): c += cost * self.weights[pcf] return c
[docs] def add_partial_costs(self, pcf: PartialCostFunction, cost: float, weight): self.partial_costs[pcf] = cost self.weights[pcf] = weight
def __str__(self): nl = "\n" t = "\t" return f"Partial costs for solution of planning problem {self.solution_id}:\n" \ f"{nl.join([p.name + ':' + t + str(self.weights[p] * c) for p, c in self.partial_costs.items()])}"
[docs]class SolutionResult: def __init__(self, benchmark_id: str, pp_results: List[PlanningProblemCostResult] = ()): """ Contains results of all solutions of a planning problem set. """ self.benchmark_id: str = benchmark_id self.total_costs: float = 0.0 self.pp_results: Dict[int, PlanningProblemCostResult] = {} for r in pp_results: self.add_results(r)
[docs] def add_results(self, pp_result: PlanningProblemCostResult): self.pp_results[pp_result.solution_id] = pp_result self.total_costs += pp_result.total_costs
def __str__(self): nl = "\n\t" return f"Total costs for benchmark {self.benchmark_id}:\n" \ f"{self.total_costs}\n" \ f"{nl.join(str(pr) for pr in self.pp_results.values())}"