Interface

This module includes the interface for sumo simulation and ego vehicle.

Sumo Simulation

SumoSimulation class

class interface.sumo_simulation.SumoSimulation[source]

Class for interfacing between the SUMO simulation and CommonRoad.

initialize(conf=None)[source]

Reads scenario files, starts traci simulation, initializes vehicles, conducts pre-simulation.

Parameters:conf (Optional[Type[SumoCommonRoadConfig]]) – configuration object. If None, use default configuration.
Return type:None
init_ego_vehicles_from_planning_problem(planning_problem_set)[source]

Initializes the ego vehicles according to planning problem set.

Parameters:planning_problem_set (PlanningProblemSet) – The planning problem set which defines the ego vehicles.
Return type:None
ego_vehicles

Returns the ego vehicles of the current simulation.

Return type:Dict[int, EgoVehicle]
current_time_step
Return type:int
Returns:current time step of interface
print_lanelet_net(with_lane_id=True, with_succ_pred=False, with_adj=False, with_speed=False)[source]

Plots commonroad road network without vehicles or obstacles.

Parameters:
  • with_lane_id – if set to true, the lane id will also be printed.
  • with_succ_pred – if set to true, the successor and predecessor lanelets will be printed.
  • with_adj – if set to true, adjacant lanelets will be printed.
  • with_speed – if set to true, the speed limit will be printed.
commonroad_scenario_at_time_step(time_step, add_ego=False, start_0=True)[source]

Creates and returns a commonroad scenario at the given time_step. Initial time_step=0 for all obstacles.

Parameters:
  • time_step (int) – the scenario will be created according this time step.
  • add_ego – whether to add ego vehicles to the scenario.
  • start_0 – if set to true, initial time step of vehicles is 0, otherwise, the current time step
Return type:

Scenario

simulate_step()[source]

Executes next simulation step (consisting of delta_steps sub-steps with dt_sumo=dt/delta_steps) in SUMO

Return type:None
get_ego_obstacles(time_step=None)[source]

Get list of ego vehicles converted to Dynamic obstacles :type time_step: Optional[int] :param time_step: initial time step, if None, get complete driven trajectory :rtype: List[DynamicObstacle] :return:

stop()[source]

Exits SUMO Simulation

check_lanelets_future_change(current_state, planned_traj)[source]

Checks the lanelet changes of the ego vehicle in the future time_window.

Parameters:
  • lanelet_network – object of the lanelet network
  • time_window – the time of the window to check the lanelet change
  • traj_index – index of the planner output corresponding to the current time step
Return type:

Tuple[str, int]

Returns:

lc_status, lc_duration: lc_status is the status of the lanelet change in the next time_window; lc_duration is the unit of time steps (using sumo dt)

check_lc_start(ego_id, lc_future_status)[source]

This function checks if a lane change is started according to the change in the lateral position and the lanelet change prediction. Note that checking the change of lateral position only is sensitive to the tiny changes, also at the boundaries of the lanes the lateral position sign is changed because it will be calculated relative to the new lane. So we check the future lanelet change to avoid these issues.

Parameters:
  • ego_id (str) – id of the ego vehicle
  • lc_future_status (str) – status of the future lanelet changes
Return type:

str

Returns:

lc_status: the status whether the ego vehicle starts a lane change or no

check_sync_mechanism(lc_status, ego_id, current_state)[source]

Defines the sync mechanism type that should be executed according to the ego vehicle motion.

Parameters:
  • lc_status (str) – status of the lanelet change in the next time_window
  • ego_id (int) – id of the ego vehicle (string)
  • current_state (State) – the current state read from the commonroad motion planner
Return type:

str

Returns:

retval: the sync mechanism that should be followed while communicating from the interface to sumo

forward_info2sumo(planned_state, sync_mechanism, lc_duration, ego_id)[source]

Forwards the information to sumo (either initiate moveToXY or changeLane) according to the sync mechanism.

Parameters:
  • planned_state (State) – the planned state from commonroad motion planner
  • sync_mechanism (str) – the sync mechanism that should be followed while communicating from the interface to sumo
  • lc_duration (int) – lane change duration, expressed in number of time steps
  • ego_id (str) – id of the ego vehicle

Ego Vehicle

EgoVehicle class

class interface.ego_vehicle.EgoVehicle(id, initial_state, delta_steps, width=2.0, length=5.0, planning_problem=None)[source]

Interface object for ego vehicle. Intended use: Get current state with EgoVehicle.current_state() and set planned trajectory with EgoVehicle.set_planned_trajectory(planned_trajectory).

set_planned_trajectory(planned_state_list)[source]

Sets planned trajectory beginning with current time step.

Parameters:planned_state_list (List[State]) – the planned trajectory
Return type:None
get_planned_trajectory

Gets planned trajectory according to the current time step

Return type:List[State]
get_dynamic_obstacle(time_step=None)[source]

If time step is false, adds complete driven trajectory and returns the dynamic obstacles. If time step is int: starts from given step and adds planned trajectory and returns the dynamic obstacles.

Parameters:time_step (Optional[int]) – initial time step of vehicle
Return type:DynamicObstacle
Returns:DynamicObstacle object of the ego vehicle.
get_planned_state(delta_step=0)[source]

Returns the planned state.

Parameters:delta_step (int) – get plannedd state after delta steps
get_state_at_timestep(time_step)[source]

Returns the state according to the given time step.

Parameters:time_step (int) – the state is returned according to this time step.
Return type:State
current_state

Returns the current state.

Return type:State
current_time_step

Returns current time step.

Return type:int
goal

Returns the goal of the planning problem.

Return type:GoalRegion
add_state(state)[source]

Adds a state to the current state dictionary.

Parameters:state (State) – the state to be added
Return type:None
driven_trajectory

Returns trajectory prediction object for driven trajectory (mainly for plotting)

Return type:TrajectoryPrediction
width

Returns the width of the ego vehicle.

Return type:float
length

Returns the length of the ego vehicle.

Return type:float
initial_state

Returns the initial state of the ego vehicle.

Return type:State

Helper Functions

interface.util.get_route_files(config_file)[source]

Returns net-file and route-files specified in the config file.

Parameters:config_file – SUMO config file (.sumocfg)
Return type:List[str]
interface.util.initialize_id_dicts(id_convention)[source]

Creates empty nested dict structure for sumo2cr and cr2sumo dicts from id_convention and returns them.

Parameters:id_convention (dict) – dict with mapping from object type to id start number
Return type:Tuple[dict, dict]
interface.util.generate_cr_id(type, sumo_id, ids_sumo2cr)[source]

Generates a new commonroad ID without adding it to any ID dictionary.

Parameters:
  • type (str) – one of the keys in params.id_convention; the type defines the first digit of the cr_id
  • sumo_id (int) – id in sumo simulation
  • ids_sumo2cr (dict) – dictionary of ids in sumo2cr
Return type:

int

interface.util.cr2sumo(cr_id, ids_cr2sumo)[source]

Gets CommonRoad ID and returns corresponding SUMO ID.

Parameters:ids_cr2sumo (dict) – dictionary of ids in cr2sumo
Return type:int
interface.util.sumo2cr(sumo_id, ids_sumo2cr)[source]

Returns corresponding CommonRoad ID according to sumo id.

Parameters:
  • sumo_id (int) – sumo id
  • ids_sumo2cr (dict) – dictionary of ids in sumo2cr.
Return type:

int

NetError class

class interface.util.NetError(len)[source]

Exception raised if there is no net-file or multiple net-files.

RouteError class

class interface.util.RouteError[source]

Exception raised if there is no route-file.

OutOfLanelet class

class interface.util.OutOfLanelet(position)[source]

Exception raised if the position of the ego vehicle is outside all lanelets.