Module Scenario

Scenario

Scenario class

class commonroad.scenario.scenario.Scenario(dt, benchmark_id)[source]

Class which describes a Scenario entity according to the CommonRoad specification. Each scenario is described by a road network consisting of lanelets (see commonroad.scenario.lanelet.LaneletNetwork) and a set of obstacles which can be either static or dynamic (see commonroad.scenario.obstacle.Obstacle).

dt

Global time step size of the time-discrete scenario.

Return type:float
benchmark_id

Unique benchmark ID of a scenario as specified in the CommonRoad XML-file.

Return type:str
lanelet_network

Road network composed of lanelets.

Return type:LaneletNetwork
dynamic_obstacles

Returns a list of all dynamic obstacles in the scenario.

Return type:List[DynamicObstacle]
static_obstacles

Returns a list of all static obstacles in the scenario.

Return type:List[StaticObstacle]
obstacles

Returns a list of all static and dynamic obstacles in the scenario.

Return type:List[Obstacle]
add_objects(scenario_object)[source]

Function to add objects, e.g., lanelets, dynamic and static obstacles, to the scenario.

Parameters:scenario_object (Union[List[Union[Obstacle, Lanelet, LaneletNetwork]], Obstacle, Lanelet, LaneletNetwork]) – object(s) to be added to the scenario
Raises:ValueError – a value error is raised if the type of scenario_object is invalid.
remove_obstacle(obstacle)[source]

Removes a static, dynamic or a list of obstacles from the scenario. If the obstacle ID is not assigned, a warning message is given.

Parameters:obstacle (Union[Obstacle, List[Obstacle]]) – obstacle to be removed
generate_object_id()[source]

Generates a unique ID which is not assigned to any object in the scenario.

Return type:int
Returns:unique object ID
occupancies_at_time_step(time_step)[source]

Returns the occupancies of all static and dynamic obstacles at a specific time step.

Parameters:time_step (int) – occupancies of obstacles at this time step
Return type:List[Occupancy]
Returns:list of occupancies of the obstacles
obstacle_by_id(obstacle_id)[source]

Finds an obstacle for a given obstacle_id

Parameters:obstacle_id (int) – ID of the queried obstacle
Return type:Optional[Obstacle]
Returns:the obstacle object if the ID exists, otherwise None
obstacles_by_role_and_type(obstacle_role=None, obstacle_type=None)[source]

Filters the obstacles by their role and type.

Parameters:
  • obstacle_role (Union[None, ObstacleRole]) – obstacle role as defined in CommonRoad, e.g., static or dynamic
  • obstacle_type (Union[None, ObstacleType]) – obstacle type as defined in CommonRoad, e.g., car, train, or bus
Return type:

List[Obstacle]

Returns:

list of all obstacles satisfying the given obstacle_role and obstacle_type

obstacles_by_position_intervals(position_intervals, obstacle_role=(<ObstacleRole.DYNAMIC: 2>, <ObstacleRole.STATIC: 1>), time_step=None)[source]

Returns obstacles which center is located within in the given x-/y-position intervals.

Parameters:
  • position_intervals (List[Interval]) – list of intervals for x- and y-coordinates [interval_x, interval_y]
  • obstacle_role (Tuple[ObstacleRole]) – tuple containing the desired obstacle roles
Return type:

List[Obstacle]

Returns:

list of obstacles in the position intervals

translate_rotate(translation, angle)[source]

Translates and rotates all objects, e.g., obstacles and road network, in the scenario.

Parameters:
  • translation (ndarray) – translation vector [x_off, y_off] in x- and y-direction
  • angle (float) – rotation angle in radian (counter-clockwise)

Road network

LaneletNetwork class

class commonroad.scenario.lanelet.LaneletNetwork[source]

Class which represents a network of connected lanelets

lanelets
Return type:List[Lanelet]
classmethod create_from_lanelet_list(lanelets)[source]

Creates a LaneletNetwork object from a given list of lanelets

Parameters:lanelets (list) – The list of lanelets
Returns:The LaneletNetwork for the given list of lanelets
classmethod create_from_lanelet_network(lanelet_network)[source]

Creates a lanelet network from a given lanelet network (copy)

Parameters:lanelet_network (LaneletNetwork) – The existing lanelet network
Returns:The deep copy of the lanelet network
find_lanelet_by_id(lanelet_id)[source]

Finds a lanelet for a given lanelet_id

Parameters:lanelet_id (int) – The id of the lanelet to find
Return type:Lanelet
Returns:The lanelet object if the id exists and None otherwise
add_lanelet(lanelet)[source]

Adds a lanelet to the LaneletNetwork

Parameters:lanelet (Lanelet) – The lanelet to add
Returns:True if the lanelet has successfully been added to the network, false otherwise
add_lanelets_from_network(lanelet_network)[source]

Adds lanelets from a given network object to the current network

Parameters:lanelet_network (LaneletNetwork) – The lanelet network
Returns:True if all lanelets have been added to the network, false otherwise
translate_rotate(translation, angle)[source]

Translates and rotates the complete lanelet network

Parameters:
  • translation (ndarray) – The translation given as [x_off,y_off] for the x and y translation
  • angle (float) – The rotation angle in radian (counter-clockwise defined)
find_lanelet_by_position(point_list)[source]

Finds the lanelet id of a given position

Parameters:point_list (List[ndarray]) – The list of positions to check
Return type:List[List[int]]
Returns:A list of lanelet ids. If the position could not be matched to a lanelet, an empty list is returned
filter_obstacles_in_network(obstacles)[source]

Returns the list of obstacles which are located in the lanelet network

Parameters:obstacles (List[Obstacle]) – The list of obstacles to check
Return type:List[Obstacle]
Returns:The list of obstacles which are located in the lanelet network
map_obstacles_to_lanelets(obstacles)[source]

Maps a given list of obstacles to the lanelets of the lanelet network

Parameters:obstacles (List[Obstacle]) – The list of CR obstacles
Return type:Dict[int, List[Obstacle]]
Returns:A dictionary with the lanelet id as key and the list of obstacles on the lanelet as a List[Obstacles]
lanelets_in_proximity(point, radius)[source]

Finds all lanelets which intersect a given circle, defined by the center point and radius

Parameters:
  • point (list) – The center of the circle
  • radius (float) – The radius of the circle
Return type:

List[Lanelet]

Returns:

The list of lanelets which intersect the given circle

Lanelet class

class commonroad.scenario.lanelet.Lanelet(left_vertices, center_vertices, right_vertices, lanelet_id, predecessor=None, successor=None, adjacent_left=None, adjacent_left_same_direction=None, adjacent_right=None, adjacent_right_same_direction=None, speed_limit=inf, line_marking_left_vertices=None, line_marking_right_vertices=None)[source]

Class which describes a Lanelet entity according to the CommonRoad specification. Each lanelet is described by a left and right boundary (polylines). Furthermore, lanelets have relations to other lanelets, e.g. an adjacent left neighbor or a predecessor.

distance
Return type:ndarray
lanelet_id
Return type:int
speed_limit
Return type:float
left_vertices
Return type:ndarray
right_vertices
Return type:ndarray
center_vertices
Return type:ndarray
line_marking_left_vertices
Return type:LineMarking
line_marking_right_vertices
Return type:LineMarking
predecessor
Return type:list
successor
Return type:list
adj_left
Return type:int
adj_left_same_direction
Return type:bool
adj_right
Return type:int
adj_right_same_direction
Return type:bool
translate_rotate(translation, angle)[source]

This method translates and rotates a lanelet

Parameters:
  • translation (ndarray) – The translation given as [x_off,y_off] for the x and y translation
  • angle (float) – The rotation angle in radian (counter-clockwise defined)
interpolate_position(distance)[source]

Computes the interpolated positions on the center/right/left polyline of the lanelet for a given distance along the lanelet

Parameters:distance (float) – The distance for the interpolation
Return type:tuple
Returns:The interpolated positions on the center/right/left polyline in the form [[x_c,y_c],[x_r,y_r],[x_l,y_l]]
convert_to_polygon()[source]

Converts the given lanelet to a polygon representation

Return type:Polygon
Returns:The polygon of the lanelet
contains_points(point_list)[source]

Checks if a list of points is enclosed in the lanelet

Parameters:point_list (ndarray) – The list of points in the form [[px1,py1],[px2,py2,],…]
Return type:List[bool]
Returns:List of bools with True indicating point is enclosed and False otherwise
get_obstacles(obstacles, time_step=0)[source]

Returns the subset of obstacles, which are located in the lanelet, of a given candidate set

Parameters:
  • obstacles (List[Obstacle]) – The set of obstacle candidates
  • time_step (int) – The time step for the occupancy to check
Return type:

List[Obstacle]

Returns:

classmethod merge_lanelets(lanelet1, lanelet2)[source]

Merges two lanelets which are in predecessor-successor relation

Parameters:
  • lanelet1 (Lanelet) – The first lanelet
  • lanelet2 (Lanelet) – The second lanelet
Return type:

Lanelet

Returns:

Merged lanelet (predecessor => successor)

classmethod all_lanelets_by_merging_successors_from_lanelet(lanelet, network, max_length=150.0)[source]

Computes all reachable lanelets starting from a provided lanelet and merges them to a single lanelet for each route.

Parameters:
  • lanelet (Lanelet) – The lanelet to start from
  • network (LaneletNetwork) – The network which contains all lanelets
  • max_length (float) – maxmimal length of merged lanelets can be provided
Return type:

Tuple[List[Lanelet], List[List[int]]]

Returns:

List of merged lanelets, Lists of lanelet ids of which each merged lanelet consists

LineMarking class

class commonroad.scenario.lanelet.LineMarking[source]

Enum describing different types of line markings, i.e. dashed or solid lines

DASHED = 1
SOLID = 2

Obstacles

Different kinds of traffic participants are modeled as obstacles within the scenario. An obstacle is either static or dynamic.

Inheritance diagram of StaticObstacle, DynamicObstacle

ObstacleRole class

class commonroad.scenario.obstacle.ObstacleRole[source]

Enum containing all possible obstacle roles defined in CommonRoad.

STATIC = 1
DYNAMIC = 2

ObstacleType class

class commonroad.scenario.obstacle.ObstacleType[source]

Enum containing all possible obstacle types defined in CommonRoad.

UNKNOWN = 0
CAR = 1
TRUCK = 2
BUS = 3
BICYCLE = 4
PEDESTRIAN = 5
PRIORITY_VEHICLE = 6
PARKED_VEHICLE = 7
CONSTRUCTION_ZONE = 8
TRAIN = 9
ROAD_BOUNDARY = 10

Obstacle class

class commonroad.scenario.obstacle.Obstacle(obstacle_id, obstacle_role, obstacle_type, obstacle_shape, initial_state)[source]

Superclass for dynamic and static obstacles holding common properties defined in CommonRoad.

initial_state

Initial state of the obstacle, e.g., obtained through sensor measurements.

Return type:State
obstacle_id

Unique ID of the obstacle.

Return type:int
obstacle_role

Obstacle role as defined in CommonRoad.

Return type:ObstacleRole
obstacle_shape

Obstacle shape as defined in CommonRoad.

Return type:Shape
obstacle_type

Obstacle type as defined in CommonRoad.

Return type:ObstacleType

StaticObstacle class

class commonroad.scenario.obstacle.StaticObstacle(obstacle_id, obstacle_type, obstacle_shape, initial_state)[source]

Class representing static obstacles as defined in CommonRoad.

initial_state

Initial state of the obstacle, e.g., obtained through sensor measurements.

Return type:State
obstacle_id

Unique ID of the obstacle.

Return type:int
obstacle_role

Obstacle role as defined in CommonRoad.

Return type:ObstacleRole
obstacle_shape

Obstacle shape as defined in CommonRoad.

Return type:Shape
obstacle_type

Obstacle type as defined in CommonRoad.

Return type:ObstacleType
occupancy_at_time(time_step)[source]

Returns the predicted occupancy of the obstacle at a specific time step.

Parameters:time_step (int) – discrete time step
Return type:Occupancy
Returns:occupancy of the static obstacle at time step
translate_rotate(translation, angle)[source]

First translates the static obstacle, then rotates the static obstacle around the origin.

Parameters:
  • translation (ndarray) – translation vector [x_off, y_off] in x- and y-direction
  • angle (float) – rotation angle in radian (counter-clockwise)

DynamicObstacle class

class commonroad.scenario.obstacle.DynamicObstacle(obstacle_id, obstacle_type, obstacle_shape, initial_state, prediction=None)[source]

Class representing dynamic obstacles as defined in CommonRoad. Each dynamic obstacle has stored its predicted movement in future time steps.

initial_state

Initial state of the obstacle, e.g., obtained through sensor measurements.

Return type:State
obstacle_id

Unique ID of the obstacle.

Return type:int
obstacle_role

Obstacle role as defined in CommonRoad.

Return type:ObstacleRole
obstacle_shape

Obstacle shape as defined in CommonRoad.

Return type:Shape
obstacle_type

Obstacle type as defined in CommonRoad.

Return type:ObstacleType
occupancy_at_time(time_step)[source]

Returns the predicted occupancy of the obstacle at a specific time step.

Parameters:time_step (int) – discrete time step
Return type:Union[None, Occupancy]
Returns:predicted occupancy of the obstacle at time step
prediction

Prediction describing the movement of the dynamic obstacle over time.

Return type:Optional[Prediction]
translate_rotate(translation, angle)[source]

First translates the dynamic obstacle, then rotates the dynamic obstacle around the origin.

Parameters:
  • translation (ndarray) – translation vector [x_off, y_off] in x- and y-direction
  • angle (float) – rotation angle in radian (counter-clockwise)

States and Trajectories

State class

class commonroad.scenario.trajectory.State(**kwargs)[source]

A state can be either exact or uncertain. Uncertain state elements can be either of type commonroad.common.util.Interval or of type commonroad.geometry.shape.Shape. A state is composed of several elements which are determined during runtime. The possible state elements are defined as slots, which comprise the necessary state elements to describe the states of all CommonRoad vehicle models:

Variables:
  • position\(s_x\)- and \(s_y\)-position in a global coordinate system. Exact positions are given as numpy array [x, y], uncertain positions are given as commonroad.geometry.shape.Shape
  • orientation – yaw angle \(\Psi\). Exact values are given as real number, uncertain values are given as commonroad.common.util.AngleInterval
  • velocity – velocity \(v_x\) in longitudinal direction in the vehicle-fixed coordinate system. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • steering_angle – steering angle \(\delta\) of front wheels. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • yaw_rate – yaw rate \(\dot{\Psi}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • slip_angle – slip angle \(\beta\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • roll_angle – roll angle \(\Phi_S\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • roll_rate – roll rate \(\dot{\Phi}_S\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • pitch_angle – pitch angle \(\Theta_S\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • pitch_rate – pitch rate \(\dot{\Theta}_S\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • velocity_y – velocity \(v_y\) in lateral direction in the vehicle-fixed coordinate system. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • position_z – position \(s_z\) (height) from ground. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • velocity_z – velocity \(v_z\) in vertical direction perpendicular to road plane. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • roll_angle_front – roll angle front \(\Phi_{UF}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • roll_rate_front – roll rate front \(\dot{\Phi}_{UF}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • velocity_y_front – velocity \(v_{y,UF}\) in y-direction front. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • position_z_front – position \(s_{z,UF}\) in z-direction front. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • velocity_z_front – velocity \(v_{z,UF}\) in z-direction front. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • roll_angle_rear – roll angle rear \(\Phi_{UR}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • roll_rate_rear – roll rate rear \(\dot{\Phi}_{UR}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • velocity_y_rear – velocity \(v_{y,UR}\) in y-direction rear. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • position_z_rear – position \(s_{z,UR}\) in z-direction rear. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • velocity_z_rear – velocity \(v_{z,UR}\) in z-direction rear. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • left_front_wheel_angular_speed – left front wheel angular speed \(\omega_{LF}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • right_front_wheel_angular_speed – right front wheel angular speed \(\omega_{RF}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • left_rear_wheel_angular_speed – left rear wheel angular speed \(\omega_{LR}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • right_rear_wheel_angular_speed – right rear wheel angular speed \(\omega_{RR}\). Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • delta_y_f – front lateral displacement \(\delta_{y,f}\) of sprung mass due to roll. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • delta_y_r – rear lateral displacement \(\delta_{y,r}\) of sprung mass due to roll. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • acceleration – acceleration \(a\). We optionally include acceleration as a state variable for obstacles to provide additional information, e.g., for motion prediction, even though acceleration is often used as an input for vehicle models. Exact values are given as real number, uncertain values are given as commonroad.common.util.Interval
  • time_step – the discrete time step. Exact values are given as integers, uncertain values are given as commonroad.common.util.Interval
Example:
>>> import numpy as np
>>> from commonroad.scenario.trajectory import State
>>> from commonroad.common.util import Interval
>>> # a state with position [2.0, 3.0] m and uncertain velocity from 5.4 to 7.0 m/s
>>> # can be created as follows:
>>> state = State(position=np.array([2.0, 3.0]), velocity=Interval(5.4, 7.0))
translate_rotate(translation, angle)[source]

First translates the state, and then rotates the state around the origin.

Parameters:
  • translation (ndarray) – translation vector [x_off, y_off] in x- and y-direction
  • angle (float) – rotation angle in radian (counter-clockwise)
Return type:

State

Returns:

transformed state

attributes

Returns all dynamically set attributes of an instance of State.

Example:
>>> import numpy as np
>>> from commonroad.scenario.trajectory import State
>>> state = State(position=np.array([0.0, 0.0]), orientation=0.1, velocity=3.4)
>>> print(state.attributes)
['position', 'orientation', 'velocity']
Return type:List[str]
Returns:subset of slots which are dynamically assigned to the object.

Trajectory class

class commonroad.scenario.trajectory.Trajectory(initial_time_step, state_list)[source]

Class to model the movement of an object over time. The states of the trajectory can be either exact or uncertain (see commonroad.scenario.trajectory.State); however, only exact time_step are allowed.

final_state

Final state of the trajectory.

Return type:State
initial_time_step

Initial time step of the trajectory.

Return type:int
state_at_time_step(time_step)[source]

Function to get the state of a trajectory at a specific time instance.

Parameters:time_step (int) – considered time step
Return type:Optional[State]
Returns:state of the trajectory at time_step
state_list

List of states of the trajectory over time.

Return type:List[State]
translate_rotate(translation, angle)[source]

First translates each state of the trajectory, then rotates each state of the trajectory around the origin.

Parameters:
  • translation (ndarray) – translation vector [x_off, y_off] in x- and y-direction
  • angle (float) – rotation angle in radian (counter-clockwise)