Commit 9f568a62 authored by Moritz Klischat's avatar Moritz Klischat
Browse files

version 2021.5

parent c5dcc358
# 2021.5
- improved speed of the SUMO co-simulation
- removed map conversion function that are now available in the commonroad scenario designer
...@@ -8,7 +8,7 @@ with open(path.join(this_directory, 'README.md'), 'r', encoding='utf-8') as f: ...@@ -8,7 +8,7 @@ with open(path.join(this_directory, 'README.md'), 'r', encoding='utf-8') as f:
setup( setup(
name='sumocr', name='sumocr',
version='2021.4', version='2021.5',
description='Python tool to interface with the SUMO traffic simulator', description='Python tool to interface with the SUMO traffic simulator',
keywords='autonomous automated vehicles driving motion planning', keywords='autonomous automated vehicles driving motion planning',
url='https://commonroad.in.tum.de/sumo-interface', url='https://commonroad.in.tum.de/sumo-interface',
......
import os import os
import sys import sys
# make sure $SUMO_HOME is in system pat # make sure $SUMO_HOME is in system pat
...@@ -10,4 +10,4 @@ if 'SUMO_HOME' in os.environ: ...@@ -10,4 +10,4 @@ if 'SUMO_HOME' in os.environ:
else: else:
sumo_installed = False sumo_installed = False
DOCKER_REGISTRY = "gitlab.lrz.de:5005/cps/sumo-interface/sumo_docker" DOCKER_REGISTRY = "gitlab.lrz.de:5005/tum-cps/commonroad-sumo-interface/sumo_docker"
...@@ -36,7 +36,7 @@ copyright = '2021, Moritz Klischat' ...@@ -36,7 +36,7 @@ copyright = '2021, Moritz Klischat'
author = 'Moritz Klischat' author = 'Moritz Klischat'
# The full version, including alpha/beta/rc tags # The full version, including alpha/beta/rc tags
release = '2021.4' release = "2021.5"
# -- General configuration --------------------------------------------------- # -- General configuration ---------------------------------------------------
...@@ -69,8 +69,8 @@ master_doc = 'index' ...@@ -69,8 +69,8 @@ master_doc = 'index'
project = 'commonroad-sumo-interface' project = 'commonroad-sumo-interface'
copyright = '2021, Moritz Klischat, Peter Kocsis' copyright = '2021, Moritz Klischat, Peter Kocsis'
author = 'Moritz Klischat' author = 'Moritz Klischat'
version = '2021.4' version = "2021.5"
release = '2021.4' release = "2021.5"
# List of patterns, relative to source directory, that match files and # List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files. # directories to ignore when looking for source files.
......
...@@ -12,7 +12,7 @@ from commonroad.scenario.trajectory import State, Trajectory ...@@ -12,7 +12,7 @@ from commonroad.scenario.trajectory import State, Trajectory
__author__ = "Moritz Klischat" __author__ = "Moritz Klischat"
__copyright__ = "TUM Cyber-Physical Systems Group" __copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"] __credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.4" __version__ = "2021.5"
__maintainer__ = "Moritz Klischat" __maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de" __email__ = "commonroad@lists.lrz.de"
__status__ = "Released" __status__ = "Released"
...@@ -37,9 +37,7 @@ class EgoVehicle: ...@@ -37,9 +37,7 @@ class EgoVehicle:
self._planned_trajectories: Dict[int, List[State]] = {} # collects trajectories from planner for every time step self._planned_trajectories: Dict[int, List[State]] = {} # collects trajectories from planner for every time step
self._current_time_step = initial_state.time_step self._current_time_step = initial_state.time_step
self.delta_steps = delta_steps self.delta_steps = delta_steps
self.planning_problem = planning_problem
if planning_problem is not None:
self.planning_problem = planning_problem
@property @property
def pp_id(self) -> int: def pp_id(self) -> int:
......
...@@ -28,21 +28,14 @@ from sumocr.sumo_config.pathConfig import SUMO_BINARY, SUMO_GUI_BINARY ...@@ -28,21 +28,14 @@ from sumocr.sumo_config.pathConfig import SUMO_BINARY, SUMO_GUI_BINARY
from xml.etree import cElementTree as ET from xml.etree import cElementTree as ET
try: try:
import traci import libsumo as traci
from traci._person import PersonDomain
from traci._vehicle import VehicleDomain
from traci._simulation import SimulationDomain
from traci._route import RouteDomain
except ModuleNotFoundError as exp: except ModuleNotFoundError as exp:
PersonDomain = DummyClass pass
VehicleDomain = DummyClass
SimulationDomain = DummyClass
RouteDomain = DummyClass
__author__ = "Moritz Klischat, Maximilian Frühauf" __author__ = "Moritz Klischat, Maximilian Frühauf"
__copyright__ = "TUM Cyber-Physical Systems Group" __copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"] __credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.4" __version__ = "2021.5"
__maintainer__ = "Moritz Klischat" __maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de" __email__ = "commonroad@lists.lrz.de"
__status__ = "Released" __status__ = "Released"
...@@ -61,10 +54,10 @@ class SumoSimulation: ...@@ -61,10 +54,10 @@ class SumoSimulation:
self.planning_problem_set: PlanningProblemSet = None self.planning_problem_set: PlanningProblemSet = None
# {time_step: {obstacle_id: state}} # {time_step: {obstacle_id: state}}
self.obstacle_states: Dict[int, Dict[int, State]] = defaultdict(lambda: dict()) self.obstacle_states: Dict[int, Dict[int, State]] = defaultdict(lambda: dict())
self.simulationdomain: SimulationDomain = SimulationDomain() self.simulationdomain = traci.simulation
self.vehicledomain: VehicleDomain = VehicleDomain() self.vehicledomain = traci.vehicle
self.persondomain: PersonDomain = PersonDomain() self.persondomain = traci.person
self.routedomain: RouteDomain = RouteDomain() self.routedomain = traci.route
self._current_time_step = 0 self._current_time_step = 0
self.ids_sumo2cr, self.ids_cr2sumo = initialize_id_dicts(ID_DICT) self.ids_sumo2cr, self.ids_cr2sumo = initialize_id_dicts(ID_DICT)
self._max_lanelet_network_id = 0 # keep track of all IDs in CR lanelet_network self._max_lanelet_network_id = 0 # keep track of all IDs in CR lanelet_network
...@@ -94,7 +87,7 @@ class SumoSimulation: ...@@ -94,7 +87,7 @@ class SumoSimulation:
return self._scenarios return self._scenarios
@scenarios.setter @scenarios.setter
def scenarios(self, scenarios: AbstractScenarioWrapper): def scenarios(self, scenarios: ScenarioWrapper):
def max_lanelet_network_id(lanelet_network: LaneletNetwork) -> int: def max_lanelet_network_id(lanelet_network: LaneletNetwork) -> int:
max_lanelet = np.max([l.lanelet_id for l in lanelet_network.lanelets]) \ max_lanelet = np.max([l.lanelet_id for l in lanelet_network.lanelets]) \
if lanelet_network.lanelets else 0 if lanelet_network.lanelets else 0
...@@ -120,7 +113,7 @@ class SumoSimulation: ...@@ -120,7 +113,7 @@ class SumoSimulation:
self._scenarios = scenarios self._scenarios = scenarios
def initialize(self, conf: DefaultConfig, def initialize(self, conf: DefaultConfig,
scenario_wrapper: AbstractScenarioWrapper, scenario_wrapper: ScenarioWrapper,
planning_problem_set: PlanningProblemSet = None) -> None: planning_problem_set: PlanningProblemSet = None) -> None:
""" """
Reads scenario files, starts traci simulation, initializes vehicles, conducts pre-simulation. Reads scenario files, starts traci simulation, initializes vehicles, conducts pre-simulation.
...@@ -137,8 +130,8 @@ class SumoSimulation: ...@@ -137,8 +130,8 @@ class SumoSimulation:
self.logger = self._init_logging() self.logger = self._init_logging()
# assert isinstance(scenario_wrapper, AbstractScenarioWrapper), \ assert isinstance(scenario_wrapper, AbstractScenarioWrapper), \
# f'scenario_wrapper expected type AbstractScenarioWrapper or None, but got type {type(scenario_wrapper)}' f'scenario_wrapper expected type ScenarioWrapper or None, but got type {type(scenario_wrapper)}'
self.scenarios = scenario_wrapper self.scenarios = scenario_wrapper
self.dt = self.conf.dt self.dt = self.conf.dt
self.dt_sumo = self.conf.dt / self.conf.delta_steps self.dt_sumo = self.conf.dt / self.conf.delta_steps
...@@ -173,10 +166,10 @@ class SumoSimulation: ...@@ -173,10 +166,10 @@ class SumoSimulation:
random.seed(self.conf.random_seed) random.seed(self.conf.random_seed)
cmd.extend(['--seed', str(self.conf.random_seed)]) cmd.extend(['--seed', str(self.conf.random_seed)])
traci.start(cmd, label=self.traci_label) traci.start(cmd)
# simulate until ego_time_start # simulate until ego_time_start
if self.planning_problem_set is not None and len(self._find_ego_vehicles_in_rou_file()) == 0: if self.planning_problem_set is not None and len(self._find_ego_vehicles_in_rou_file()) == 0:
self.__presimulation_silent(self.conf.presimulation_steps - 1, fetch_obstacles=False) self.__presimulation_silent(max(0, self.conf.presimulation_steps-1), fetch_obstacles=False)
if len(self.ego_vehicles) > 0: if len(self.ego_vehicles) > 0:
self.logger.warning('<SumoSimulation/init_ego_vehicles> Ego vehicles are already defined through .rou' self.logger.warning('<SumoSimulation/init_ego_vehicles> Ego vehicles are already defined through .rou'
'file and planning problem!') 'file and planning problem!')
...@@ -233,7 +226,7 @@ class SumoSimulation: ...@@ -233,7 +226,7 @@ class SumoSimulation:
self.dummy_ego_simulation = False self.dummy_ego_simulation = False
# retrieve arbitrary route id for initialization (will not be used by interface) # retrieve arbitrary route id for initialization (will not be used by interface)
generic_route_id = self.routedomain.getIDList()[0] generic_route_id = traci.route.getIDList()[0]
width = self.conf.ego_veh_width width = self.conf.ego_veh_width
length = self.conf.ego_veh_length length = self.conf.ego_veh_length
...@@ -247,7 +240,7 @@ class SumoSimulation: ...@@ -247,7 +240,7 @@ class SumoSimulation:
sumo_id = EGO_ID_START + str(new_id) sumo_id = EGO_ID_START + str(new_id)
cr_id = self._create_cr_id(type='egoVehicle', sumo_id=sumo_id, sumo_prefix=EGO_ID_START) cr_id = self._create_cr_id(type='egoVehicle', sumo_id=sumo_id, sumo_prefix=EGO_ID_START)
self.vehicledomain.add(sumo_id, generic_route_id, typeID="DEFAULT_VEHTYPE", self.vehicledomain.add(sumo_id, generic_route_id, typeID="DEFAULT_VEHTYPE",
depart=None, depart="now",
departLane='first', departPos="base", departLane='first', departPos="base",
departSpeed=planning_problem.initial_state.velocity, departSpeed=planning_problem.initial_state.velocity,
arrivalLane="current", arrivalPos="max", arrivalLane="current", arrivalPos="max",
...@@ -259,9 +252,9 @@ class SumoSimulation: ...@@ -259,9 +252,9 @@ class SumoSimulation:
position_tmp = planning_problem.initial_state.position\ position_tmp = planning_problem.initial_state.position\
+ 0.5 * length * np.array([math.cos(planning_problem.initial_state.orientation), + 0.5 * length * np.array([math.cos(planning_problem.initial_state.orientation),
math.sin(planning_problem.initial_state.orientation)]) math.sin(planning_problem.initial_state.orientation)])
self.vehicledomain.setLength(vehID=sumo_id, length=length) self.vehicledomain.setLength(sumo_id, length=length)
self.vehicledomain.setWidth(vehID=sumo_id, width=width) self.vehicledomain.setWidth(sumo_id, width=width)
self.vehicledomain.moveToXY(vehID=sumo_id, edgeID='dummy', lane=-1, self.vehicledomain.moveToXY(sumo_id, edgeID='dummy', laneIndex=-1,
x=position_tmp[0], x=position_tmp[0],
y=position_tmp[1], y=position_tmp[1],
angle=sumo_angle, keepRoute=2) angle=sumo_angle, keepRoute=2)
...@@ -281,18 +274,11 @@ class SumoSimulation: ...@@ -281,18 +274,11 @@ class SumoSimulation:
:param ego_vehicle: the ego vehicle to be added. :param ego_vehicle: the ego vehicle to be added.
""" """
if ego_vehicle.id in self._ego_vehicles: if ego_vehicle.id in self._ego_vehicles:
self.logger.error( self.logger.debug(
f'Ego vehicle with id {ego_vehicle.id} already exists!', f'Ego vehicle with id {ego_vehicle.id} already exists!',
exc_info=True) exc_info=True)
else:
self._ego_vehicles[ego_vehicle.id] = ego_vehicle self._ego_vehicles[ego_vehicle.id] = ego_vehicle
@property
def traci_label(self):
"""Unique label to identify simulation"""
if self._traci_label is None:
self._traci_label = self.conf.scenario_name + time.strftime("%H%M%S") + str(random.randint(0, 100))
return self._traci_label
@property @property
def ego_vehicles(self) -> Dict[int, EgoVehicle]: def ego_vehicles(self) -> Dict[int, EgoVehicle]:
...@@ -441,20 +427,42 @@ class SumoSimulation: ...@@ -441,20 +427,42 @@ class SumoSimulation:
self._fetch_sumo_vehicles(self.current_time_step, self.vehicledomain.getIDList()) self._fetch_sumo_vehicles(self.current_time_step, self.vehicledomain.getIDList())
self._fetch_sumo_pedestrians(self.current_time_step, self.persondomain.getIDList()) self._fetch_sumo_pedestrians(self.current_time_step, self.persondomain.getIDList())
def _subscribe_new(self, new_ids: List[str], domain=traci.vehicle):
"""
Subscribe to values of new vehicle/pedestrian
:param new_ids: list of new SUMO IDs
:param domain: domain from which should be subscribed
:return:
"""
vehicle_ids = self.sort_ego_first(new_ids)
for veh_id in vehicle_ids:
domain.subscribe(veh_id, traci_subscription_values)
def _get_subscription_results(self, domain=traci.vehicle) -> Dict[str, Dict[int, any]]:
"""
Return subscription results from current time step
:param domain:
:return: dict with results with structure {sumo_id: {value_id: value}}
"""
return domain.getAllSubscriptionResults()
def _fetch_sumo_vehicles(self, time_step: int, new_ids: List[str]): def _fetch_sumo_vehicles(self, time_step: int, new_ids: List[str]):
""" """
Gets and stores all vehicle states from SUMO. Initializes ego vehicles when they enter simulation. Gets and stores all vehicle states from SUMO. Initializes ego vehicles when they enter simulation.
""" """
vehicle_ids = self.sort_ego_first(self.vehicledomain.getIDList()) self._subscribe_new(new_ids)
if len(vehicle_ids) == 0: vehicle_states = self._get_subscription_results()
if len(vehicle_states) == 0:
return return
if not self.dummy_ego_simulation: if not self.dummy_ego_simulation:
self.check_ego_collisions(raise_error=True) self.check_ego_collisions(raise_error=True)
for veh_id in vehicle_ids: for veh_id, vehicle_state in vehicle_states.items():
state = self._get_current_state_from_sumo(self.vehicledomain, str(veh_id), SUMO_VEHICLE_PREFIX) state = self._extract_current_state(self.vehicledomain, veh_id, vehicle_state,
SUMO_VEHICLE_PREFIX)
if state is None: if state is None:
continue continue
...@@ -470,6 +478,8 @@ class SumoSimulation: ...@@ -470,6 +478,8 @@ class SumoSimulation:
if self.planning_problem_set is not None: if self.planning_problem_set is not None:
planning_problem = list(self.planning_problem_set.planning_problem_dict.values())[0] planning_problem = list(self.planning_problem_set.planning_problem_dict.values())[0]
elif self.scenarios.planning_problem_set is not None:
planning_problem = list(self.scenarios.planning_problem_set.planning_problem_dict.values())[0]
else: else:
planning_problem = None planning_problem = None
...@@ -492,7 +502,7 @@ class SumoSimulation: ...@@ -492,7 +502,7 @@ class SumoSimulation:
# read signal state # read signal state
if not self._silent: if not self._silent:
signal_states = get_signal_state(self.vehicledomain.getSignals(veh_id), self.current_time_step) signal_states = get_signal_state(vehicle_state[traci.constants.VAR_SIGNALS], self.current_time_step)
key = self.ids_sumo2cr['obstacleVehicle'][veh_id] \ key = self.ids_sumo2cr['obstacleVehicle'][veh_id] \
if veh_id in self.ids_sumo2cr['obstacleVehicle'] else self.ids_sumo2cr[EGO_ID_START][veh_id] if veh_id in self.ids_sumo2cr['obstacleVehicle'] else self.ids_sumo2cr[EGO_ID_START][veh_id]
self.signal_states[key].append(signal_states) self.signal_states[key].append(signal_states)
...@@ -516,14 +526,15 @@ class SumoSimulation: ...@@ -516,14 +526,15 @@ class SumoSimulation:
Gets and stores all vehicle states from SUMO. Initializes ego vehicles when they enter simulation. Gets and stores all vehicle states from SUMO. Initializes ego vehicles when they enter simulation.
""" """
person_ids = self.persondomain.getIDList() self._subscribe_new(new_ids, domain=traci.person)
person_states = self._get_subscription_results(domain=traci.person)
if not person_ids: if not person_states:
return return
obstacle_type = "obstaclePedestrian" obstacle_type = "obstaclePedestrian"
for ped_id in person_ids: for ped_id, ped_state in person_states.items():
state = self._get_current_state_from_sumo(self.persondomain, str(ped_id), SUMO_PEDESTRIAN_PREFIX) state = self._extract_current_state(self.persondomain, ped_id, ped_state, SUMO_PEDESTRIAN_PREFIX)
if state is None: if state is None:
continue continue
if ped_id in new_ids: if ped_id in new_ids:
...@@ -539,7 +550,7 @@ class SumoSimulation: ...@@ -539,7 +550,7 @@ class SumoSimulation:
# get obstacle vehicle state # get obstacle vehicle state
self.obstacle_states[time_step][self.ids_sumo2cr[obstacle_type][ped_id]] = state self.obstacle_states[time_step][self.ids_sumo2cr[obstacle_type][ped_id]] = state
def _get_veh_shape(self, vehicledomain: VehicleDomain, sumo_id): def _get_veh_shape(self, vehicledomain, sumo_id):
return Rectangle(vehicledomain.getLength(sumo_id), vehicledomain.getWidth(sumo_id)) return Rectangle(vehicledomain.getLength(sumo_id), vehicledomain.getWidth(sumo_id))
def check_ego_collisions(self, raise_error=True): def check_ego_collisions(self, raise_error=True):
...@@ -597,8 +608,8 @@ class SumoSimulation: ...@@ -597,8 +608,8 @@ class SumoSimulation:
finally: finally:
return Rectangle(length, width) return Rectangle(length, width)
def _get_current_state_from_sumo(self, domain, veh_id: str, def _extract_current_state(self, domain, veh_id: str, vehicle_state: Dict[int, any],
sumo_prefix: str) -> Union[None, State]: sumo_prefix: str) -> Union[None, State]:
""" """
Gets the current state from sumo. Gets the current state from sumo.
:param domain :param domain
...@@ -608,8 +619,8 @@ class SumoSimulation: ...@@ -608,8 +619,8 @@ class SumoSimulation:
:return: the state of the given vehicle or None, if not in field of view :return: the state of the given vehicle or None, if not in field of view
""" """
unique_id = sumo_prefix + veh_id unique_id = sumo_prefix + veh_id
position = np.array(domain.getPosition(veh_id)) position = np.array(vehicle_state[traci.constants.VAR_POSITION])
velocity = domain.getSpeed(veh_id) velocity = math.sqrt(vehicle_state[traci.constants.VAR_SPEED]**2 + vehicle_state[traci.constants.VAR_SPEED_LAT]**2)
cr_id = sumo2cr(veh_id, self.ids_sumo2cr) cr_id = sumo2cr(veh_id, self.ids_sumo2cr)
if cr_id and cr_id in self.obstacle_shapes: if cr_id and cr_id in self.obstacle_shapes:
...@@ -624,7 +635,7 @@ class SumoSimulation: ...@@ -624,7 +635,7 @@ class SumoSimulation:
delta_pos = position - self.cached_position[unique_id] delta_pos = position - self.cached_position[unique_id]
orientation = math.atan2(delta_pos[1], delta_pos[0]) orientation = math.atan2(delta_pos[1], delta_pos[0])
else: else:
orientation = math.radians(-domain.getAngle(veh_id) + 90) orientation = math.radians(-vehicle_state[traci.constants.VAR_ANGLE] + 90.0)
self.cached_position[unique_id] = position self.cached_position[unique_id] = position
position -= 0.5 * length * np.array([math.cos(orientation), math.sin(orientation)]) position -= 0.5 * length * np.array([math.cos(orientation), math.sin(orientation)])
...@@ -633,10 +644,7 @@ class SumoSimulation: ...@@ -633,10 +644,7 @@ class SumoSimulation:
if in_fov is False: if in_fov is False:
return None return None
try: acceleration = vehicle_state[traci.constants.VAR_ACCELERATION]
acceleration = domain.getAcceleration(veh_id)
except AttributeError:
acceleration = 0
return State(position=position, return State(position=position,
orientation=orientation, orientation=orientation,
...@@ -825,7 +833,7 @@ class SumoSimulation: ...@@ -825,7 +833,7 @@ class SumoSimulation:
Generates a new unused id for SUMO Generates a new unused id for SUMO
:return: :return:
""" """
id_list = self.vehicledomain.getIDList() id_list = traci.vehicle.getIDList()
new_id = int(len(id_list)) new_id = int(len(id_list))
i = 0 i = 0
while i < 1000: while i < 1000:
...@@ -1039,7 +1047,7 @@ class SumoSimulation: ...@@ -1039,7 +1047,7 @@ class SumoSimulation:
self.vehicledomain.moveToXY(vehID=id_sumo, self.vehicledomain.moveToXY(vehID=id_sumo,
edgeID='dummy', edgeID='dummy',
lane=-1, laneIndex=-1,
x=position_sumo[0], x=position_sumo[0],
y=position_sumo[1], y=position_sumo[1],
angle=sumo_angle, angle=sumo_angle,
......
...@@ -3,6 +3,7 @@ import xml.etree.ElementTree as et ...@@ -3,6 +3,7 @@ import xml.etree.ElementTree as et
import warnings import warnings
import libsumo as traci
from sumocr.sumo_config import plot_params, ID_DICT from sumocr.sumo_config import plot_params, ID_DICT
from typing import List, Tuple, Set from typing import List, Tuple, Set
from sumocr.sumo_config.default import SUMO_PEDESTRIAN_PREFIX, SUMO_VEHICLE_PREFIX from sumocr.sumo_config.default import SUMO_PEDESTRIAN_PREFIX, SUMO_VEHICLE_PREFIX
...@@ -12,7 +13,7 @@ import enum ...@@ -12,7 +13,7 @@ import enum
__author__ = "Moritz Klischat" __author__ = "Moritz Klischat"
__copyright__ = "TUM Cyber-Physical Systems Group" __copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"] __credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.4" __version__ = "2021.5"
__maintainer__ = "Moritz Klischat" __maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de" __email__ = "commonroad@lists.lrz.de"
__status__ = "Released" __status__ = "Released"
...@@ -151,7 +152,7 @@ def cr2sumo(cr_id: int, ids_cr2sumo: dict) -> int: ...@@ -151,7 +152,7 @@ def cr2sumo(cr_id: int, ids_cr2sumo: dict) -> int:
# raise ValueError('Commonroad id {0} does not exist.'.format(cr_id)) # raise ValueError('Commonroad id {0} does not exist.'.format(cr_id))
def sumo2cr(sumo_id: int, ids_sumo2cr: dict) -> int: def sumo2cr(sumo_id: str, ids_sumo2cr: dict) -> int:
""" """
Returns corresponding CommonRoad ID according to sumo id. Returns corresponding CommonRoad ID according to sumo id.
...@@ -173,13 +174,15 @@ def sumo2cr(sumo_id: int, ids_sumo2cr: dict) -> int: ...@@ -173,13 +174,15 @@ def sumo2cr(sumo_id: int, ids_sumo2cr: dict) -> int:
# raise ValueError('Sumo id \'%s\' does not exist.' % sumo_id) # raise ValueError('Sumo id \'%s\' does not exist.' % sumo_id)
class DummyClass: traci_subscription_values = (traci.constants.VAR_POSITION,
"""Dummy class used if SUMO is not installed and traci cannot be imported""" traci.constants.VAR_SPEED,
def __init__(self): traci.constants.VAR_SPEED_LAT,
pass traci.constants.VAR_ACCELERATION,
traci.constants.VAR_ANGLE,
traci.constants.VAR_SIGNALS)
class SumoSignalIndices(enum.Enum): class SumoSignalIndices(enum.IntEnum):
"""All interpretations with their respective bit indices """All interpretations with their respective bit indices
ref.: https://sumo.dlr.de/docs/TraCI/Vehicle_Signalling.html""" ref.: https://sumo.dlr.de/docs/TraCI/Vehicle_Signalling.html"""
VEH_SIGNAL_BLINKER_RIGHT = 0 VEH_SIGNAL_BLINKER_RIGHT = 0
...@@ -198,6 +201,9 @@ class SumoSignalIndices(enum.Enum): ...@@ -198,6 +201,9 @@ class SumoSignalIndices(enum.Enum):
VEH_SIGNAL_EMERGENCY_YELLOW = 13 VEH_SIGNAL_EMERGENCY_YELLOW = 13
max_signal_index: int = max([s.value for s in SumoSignalIndices])
_defined_signals = { _defined_signals = {
# only the following signals are computed on every time step # only the following signals are computed on every time step
SumoSignalIndices.VEH_SIGNAL_BLINKER_LEFT: "indicator_left", SumoSignalIndices.VEH_SIGNAL_BLINKER_LEFT: "indicator_left",
...@@ -212,15 +218,12 @@ def get_signal_state(state: int, time_step: int) -> SignalState: ...@@ -212,15 +218,12 @@ def get_signal_state(state: int, time_step: int) -> SignalState:
Computes the CR Signal state from the sumo signals Computes the CR Signal state from the sumo signals
""" """
binary = list(reversed(bin(state)[2:])) binary = list(reversed(bin(state)[2:]))
max_signal_index: int = max([s.value for s in SumoSignalIndices])
bit_string: List[bool] = [binary[i] == "1" if i < len(binary) else False bit_string: List[bool] = [binary[i] == "1" if i < len(binary) else False
for i in range(max_signal_index + 1)] for i in range(max_signal_index + 1)]
args = {cr_name: bit_string[sumo_name.value] args = {cr_name: bit_string[sumo_name.value]
for sumo_name, cr_name in _defined_signals.items()} for sumo_name, cr_name in _defined_signals.items()}
return SignalState(**{**args, return SignalState(**{**args,
# the following are not modelled by sumo, so have to be inserted manually
**{"horn": False, "hazard_warning_lights": False},
**{"time_step": time_step}}) **{"time_step": time_step}})
......
...@@ -4,15 +4,48 @@ from commonroad.scenario.lanelet import LaneletNetwork ...@@ -4,15 +4,48 @@ from commonroad.scenario.lanelet import LaneletNetwork
__author__ = "Moritz Klischat" __author_