Allow precision and lateral planning to be configured by vehicle/behavior

[rodrigoqueiroz]

Currently, the level of precision in trajectory planning (feasibility and cost) is based on a Global Configuration in SimConfig:

TRAJECTORY_SPLIT = 10 #(10 to 100)

In simulations with multiple vehicles, this value can be reduced, but the quality of the overall planning will be reduced (for example: slightly different trajectories not distinguishable from each other and resulting in the not optimal choice. This is acceptable depending on the scenario. The goal is to use a light configuration as a general solution, and allow specific vehicles or maneuvers to increase the precision as needed by scenario.

The second is lateral sampling. Currently, the lateral sampling is based on the LaneConfig class and can be adjusted as a global configuration to cover the entire lane space. The goal is to use a flexible configuration that can be also defined by vehicle/behavior or only in part of a scenario. For example, if no obstacle avoidance is needed, the lateral sampling can be simplified to target only the centre of the lane.

This issue is related to Jump Back issue #41 , because it has a big impact in performance.

[rodrigoqueiroz]

Solution 1:

Precision is now defined inside the maneuver config in ManeuverConfig.py

#Precision defines how feasibility and costs are computed (and how integrals are approximated). 
#Higher(100) = better precision, but impacts performance. 
#Use with caution when multiple vehicles are used in simulation

cost_precision:float = 10             #from 10 to 100.

As a general maneuver calibration option, the precision can be adjusted in the behavior tree when needed. This allows less important vehicles to perform non-optimal planning, while key vehicles or moments in a scenario can switch the precision level.

maneuver m_keepvelocity ( MVelKeepConfig( vel=MP(10.0,10,6), cost_precision=100 ) )

->
    condition c_busy_lane( lane_occupied() )
    maneuver m_follow_lead ( MFollowConfig(cost_precision=50)) )
maneuver m_keepvelocity ( MVelKeepConfig( vel=MP(10.0,10,6), cost_precision=10 )  

[rodrigoqueiroz]

Solution 2: Lateral Sampling

A New class LT is used to config the lateral planning

class LT:
    '''
    Lateral target configuration for sampling in the lane width space
    '''
    target:float = 0.0                  #lateral target position in meters (0.0 = center of the lane)
    nsamples:int = 1                    #number of sampling points on the lane width for lateral planning
    sampling:int = SamplingMethod.NORMAL
    sigma:float = 1.0                   #std dev for sampling from normal
    limit_lane_width:bool = True       #if True, all sampling values will be limited by lane width
    limit_vehicle_size:bool = True      #if True, sampling values will be limited by vehicle size

It is part of the MConfig, so every maneuver can switch to its own target and sampling configuration. By default, targets exclusively the centre (0.0), 1 sample

#Lateral lane target. 
lat_target:LT = LT(0.0,1)

Can be configured per behavior tree. Example1: Targets centre, with 3 samples around target using a gaussian (default) `maneuver m_keepvelocity ( MVelKeepConfig(lat_target=LT( 0.0, 3)))

Example2: Targets left of lane (0.1m), with 3 samples around target using a gaussian (default) maneuver m_keepvelocity ( MVelKeepConfig( lat_target=LT( 0.1, 3)))

Example3: Targets center lane, with 4 equally distributed positions along the lane width with SamplingMethod.LINEAR (1) This is ideal for obstacle avoidance, covering the entire width and selecting trajectories away from them.

maneuver m_keepvelocity ( MVelKeepConfig( lat_target=LT(0.0, 4, 1)))

Example4: Targets right of lane and drifts into right lane space (if exists), with 4 samples using SamplingMethod.NORMAL (3) Note how this also requires the feasibility constraint 'off_lane' to be set to 0. Otherwise, the vehicle will plan maneuvers crossing the boundaries, but won't select them.

maneuver m_keepvelocity ( MVelKeepConfig( lat_target=LT(0.0,6,3), feasibility_constraints['off_lane'] = 0

[rodrigoqueiroz]

The solutions also require multiple changes in the CostFunctions and ManeuverModels that I won't detail. Both are implemented and tested.

Current trees still work. They will follow the standard behavior, unless the new options are inserted into the trees or GeoScenario files.