Commit ebfd5f30 authored by Moritz Klischat's avatar Moritz Klischat
Browse files

fix obstacles not fetched at initial time step

parent a66808ad
......@@ -8,7 +8,7 @@ with open(path.join(this_directory, 'README.md'), 'r', encoding='utf-8') as f:
setup(
name='sumocr',
version='2021.2',
version='2021.3',
description='Python tool to interface with the SUMO traffic simulator',
keywords='autonomous automated vehicles driving motion planning',
url='https://commonroad.in.tum.de/sumo-interface',
......
......@@ -10,4 +10,4 @@ if 'SUMO_HOME' in os.environ:
else:
sumo_installed = False
DOCKER_REGISTRY = "gitlab.lrz.de:5005/tum-cps/commonroad-sumo-interface/sumo_docker"
DOCKER_REGISTRY = "gitlab.lrz.de:5005/cps/sumo-interface/sumo_docker"
......@@ -36,7 +36,7 @@ copyright = '2021, Moritz Klischat'
author = 'Moritz Klischat'
# The full version, including alpha/beta/rc tags
release = '2021.2'
release = '2021.3'
# -- General configuration ---------------------------------------------------
......@@ -69,8 +69,8 @@ master_doc = 'index'
project = 'commonroad-sumo-interface'
copyright = '2021, Moritz Klischat, Peter Kocsis'
author = 'Moritz Klischat'
version = '2021.2'
release = '2021.2'
version = '2021.3'
release = '2021.3'
# List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files.
......
......@@ -12,7 +12,7 @@ from commonroad.scenario.trajectory import State, Trajectory
__author__ = "Moritz Klischat"
__copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.2"
__version__ = "2021.3"
__maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de"
__status__ = "Released"
......@@ -39,12 +39,6 @@ class EgoVehicle:
self.delta_steps = delta_steps
if planning_problem is not None:
assert np.all(np.isclose(self.initial_state.position, planning_problem.initial_state.position)), \
'<sumo_simulation/ego_vehicle> planning problem.initial_state must equal initial_state'
assert np.isclose(self.initial_state.orientation, planning_problem.initial_state.orientation), \
'<sumo_simulation/ego_vehicle> planning problem.initial_state must equal initial_state'
assert np.isclose(self.initial_state.velocity, planning_problem.initial_state.velocity), \
'<sumo_simulation/ego_vehicle> planning problem.initial_state must equal initial_state'
self.planning_problem = planning_problem
@property
......
......@@ -42,7 +42,7 @@ except ModuleNotFoundError as exp:
__author__ = "Moritz Klischat, Maximilian Frühauf"
__copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.2"
__version__ = "2021.3"
__maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de"
__status__ = "Released"
......@@ -146,15 +146,11 @@ class SumoSimulation:
self.planning_problem_set = planning_problem_set \
if planning_problem_set is not None else self.scenarios.planning_problem_set
np.random.seed(self.conf.random_seed)
assert sumocr.sumo_installed, "SUMO not installed or environment variable SUMO_HOME not set."
if self.conf.with_sumo_gui:
self.logger.warning(
'Continuous lane change currently not implemented for sumo-gui.'
)
cmd = [
SUMO_GUI_BINARY, "-c", self.scenarios.sumo_cfg_file,
SUMO_GUI_BINARY, "--start", "-c", self.scenarios.sumo_cfg_file,
"--step-length",
str(self.dt_sumo), "--ignore-route-errors",
"--lateral-resolution",
......@@ -178,16 +174,17 @@ class SumoSimulation:
traci.start(cmd, label=self.traci_label)
# simulate until ego_time_start
self.__presimulation_silent(self.conf.presimulation_steps)
self.initialized = True
# initializes ego vehicle positions if not defined in .rou file
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)
if len(self.ego_vehicles) > 0:
self.logger.warning('<SumoSimulation/init_ego_vehicles> Ego vehicles are already defined through .rou'
'file and planning problem!')
self.initialized = True
self.init_ego_vehicles_from_planning_problem(self.planning_problem_set)
self.__presimulation_silent(1, fetch_obstacles=True)
else:
self.__presimulation_silent(self.conf.presimulation_steps, fetch_obstacles=True)
self.initialized = True
def _init_logging(self):
# Create a custom logger
......@@ -261,13 +258,12 @@ class SumoSimulation:
position_tmp = planning_problem.initial_state.position\
+ 0.5 * length * np.array([math.cos(planning_problem.initial_state.orientation),
math.sin(planning_problem.initial_state.orientation)])
self.vehicledomain.setLength(vehID=sumo_id,length=length)
self.vehicledomain.setLength(vehID=sumo_id, length=length)
self.vehicledomain.setWidth(vehID=sumo_id, width=width)
self.vehicledomain.moveToXY(vehID=sumo_id, edgeID='dummy', lane=-1,
x=position_tmp[0],
y=position_tmp[1],
angle=sumo_angle, keepRoute=2)
self.__presimulation_silent(1)
self.ids_sumo2cr[EGO_ID_START][sumo_id] = cr_id
self.ids_sumo2cr['vehicle'][sumo_id] = cr_id
......@@ -387,11 +383,13 @@ class SumoSimulation:
self._fetch_sumo_vehicles(self.current_time_step)
self._fetch_sumo_pedestrians(self.current_time_step)
def __presimulation_silent(self, pre_simulation_steps: int):
def __presimulation_silent(self, pre_simulation_steps: int, fetch_obstacles=True):
"""
Simulate SUMO without synchronization of interface. Used before starting interface simulation.
:param pre_simulation_steps: the steps of simulation which are executed before checking the existence of ego vehicles and configured simulation step.
:param pre_simulation_steps: the steps of simulation which are executed before checking the existence
of ego vehicles and configured simulation step.
:param fetch_obstacles: fetch obstacles after simulating
"""
assert self.current_time_step == 0
......@@ -405,8 +403,9 @@ class SumoSimulation:
traci.simulationStep()
self._silent = False
self._fetch_sumo_vehicles(self.current_time_step)
self._fetch_sumo_pedestrians(self.current_time_step)
if fetch_obstacles:
self._fetch_sumo_vehicles(self.current_time_step)
self._fetch_sumo_pedestrians(self.current_time_step)
def _fetch_sumo_vehicles(self, time_step: int):
"""
......@@ -1044,12 +1043,12 @@ class SumoSimulation:
for ego_vehicle in self.ego_vehicles.values():
try:
if np.linalg.norm(ego_vehicle.get_state_at_timestep(self.current_time_step-1).position - position,
if np.linalg.norm(ego_vehicle.get_state_at_timestep(self.current_time_step).position - position,
ord=np.inf) < self.conf.field_of_view:
return True
except KeyError:
if len(ego_vehicle._state_dict.keys()) == 0:
return False
raise ValueError
return False
......
......@@ -12,7 +12,7 @@ import enum
__author__ = "Moritz Klischat"
__copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.2"
__version__ = "2021.3"
__maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de"
__status__ = "Released"
......
......@@ -4,7 +4,7 @@ from commonroad.scenario.lanelet import LaneletNetwork
__author__ = "Moritz Klischat"
__copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.2"
__version__ = "2021.3"
__maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de"
__status__ = "Released"
......
......@@ -26,7 +26,7 @@ except ImportError:
__author__ = "Moritz Klischat"
__copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.2"
__version__ = "2021.3"
__maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de"
__status__ = "Released"
......
......@@ -19,7 +19,7 @@ from sumocr.sumo_config import DefaultConfig
__author__ = "Moritz Klischat"
__copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["ZIM Projekt ZF4086007BZ8"]
__version__ = "2021.2"
__version__ = "2021.3"
__maintainer__ = "Moritz Klischat"
__email__ = "commonroad@lists.lrz.de"
__status__ = "Released"
......
......@@ -46,6 +46,8 @@ def create_video(scenario: Scenario,
frame_count_padding = int(1 / scenario.dt)
frame_count = max([obstacle.prediction.final_time_step for obstacle in scenario.obstacles]) + frame_count_padding
dynamic_obstacles_ego = []
if trajectory_pred is None:
trajectory_pred = []
if len(trajectory_pred) > 0 and isinstance(list(trajectory_pred.values())[0], EgoVehicle):
trajectories_tmp = []
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment