improve initialization and remove option --ignore-route-errors

setup.py

@@ -8,7 +8,7 @@ with open(path.join(this_directory, 'README.md'), 'r', encoding='utf-8') as f:
 
 setup(
     name='sumocr',
-    version='2021.3',
+    version='2021.4',
     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',

sumocr/docs/source/conf.py

@@ -36,7 +36,7 @@ copyright = '2021, Moritz Klischat'
 author = 'Moritz Klischat'
 
 # The full version, including alpha/beta/rc tags
-release = '2021.3'
+release = '2021.4'
 
 
 # -- General configuration ---------------------------------------------------
@@ -69,8 +69,8 @@ master_doc = 'index'
 project = 'commonroad-sumo-interface'
 copyright = '2021, Moritz Klischat, Peter Kocsis'
 author = 'Moritz Klischat'
-version = '2021.3'
-release = '2021.3'
+version = '2021.4'
+release = '2021.4'
 
 # List of patterns, relative to source directory, that match files and
 # directories to ignore when looking for source files.

sumocr/interface/ego_vehicle.py

@@ -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.3"
+__version__ = "2021.4"
 __maintainer__ = "Moritz Klischat"
 __email__ = "commonroad@lists.lrz.de"
 __status__ = "Released"

sumocr/interface/sumo_simulation.py

@@ -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.3"
+__version__ = "2021.4"
 __maintainer__ = "Moritz Klischat"
 __email__ = "commonroad@lists.lrz.de"
 __status__ = "Released"
@@ -152,7 +152,7 @@ class SumoSimulation:
             cmd = [
                 SUMO_GUI_BINARY, "--start", "-c", self.scenarios.sumo_cfg_file,
                 "--step-length",
-                str(self.dt_sumo), "--ignore-route-errors",
+                str(self.dt_sumo),
                 "--lateral-resolution",
                 str(self.conf.lateral_resolution)
             ]
@@ -160,7 +160,7 @@ class SumoSimulation:
             cmd = [
                 SUMO_BINARY, "-c", self.scenarios.sumo_cfg_file,
                 "--step-length",
-                str(self.dt_sumo), "--ignore-route-errors",
+                str(self.dt_sumo),
                 "--lateral-resolution",
                 str(self.conf.lateral_resolution)
             ]
@@ -170,6 +170,7 @@ class SumoSimulation:
 
         if self.conf.random_seed:
             np.random.seed(self.conf.random_seed)
+            random.seed(self.conf.random_seed)
             cmd.extend(['--seed', str(self.conf.random_seed)])
 
         traci.start(cmd, label=self.traci_label)
@@ -361,6 +362,37 @@ class SumoSimulation:
         self.cr_scenario.add_objects(self._get_cr_obstacles_all())
         return self.cr_scenario
 
+    def _init_vehicle_ids(self) -> Tuple[list, list]:
+        """
+        Initialize vehicle IDs and set individual vehicle parameters.
+        :return lists of new vehicle and pedestrian IDs that entered the simulation
+        """
+        new_ids = []
+        vehicle_ids = self.vehicledomain.getIDList()
+        for veh_id in vehicle_ids:
+            if veh_id not in self.ids_sumo2cr[SUMO_VEHICLE_PREFIX]:
+                if veh_id.startswith(EGO_ID_START):
+                    obs_type = EGO_ID_START
+                else:
+                    obs_type = 'obstacleVehicle'
+                self._create_cr_id(obs_type, veh_id, SUMO_VEHICLE_PREFIX)
+                vehicle_class = VEHICLE_TYPE_SUMO2CR[self.vehicledomain.getVehicleClass(str(veh_id))]
+                self._set_veh_params(self.vehicledomain, veh_id, vehicle_class)
+                new_ids.append(veh_id)
+
+        person_ids = self.persondomain.getIDList()
+        obstacle_type = "obstaclePedestrian"
+        new_ped_ids = []
+        for ped_id in person_ids:
+            if ped_id not in self.ids_sumo2cr[SUMO_PEDESTRIAN_PREFIX]:
+                # initialize new pedestrian
+                self._create_cr_id(obstacle_type, ped_id, SUMO_PEDESTRIAN_PREFIX)
+                vehicle_class = VEHICLE_TYPE_SUMO2CR[self.persondomain.getTypeID(ped_id).split("@")[0]]
+                self._set_veh_params(self.persondomain, ped_id, vehicle_class)
+                new_ped_ids.append(ped_id)
+
+        return new_ids, new_ped_ids
+
     def simulate_step(self) -> None:
         """
         Executes next simulation step (consisting of delta_steps sub-steps with dt_sumo=dt/delta_steps) in SUMO
@@ -380,8 +412,9 @@ class SumoSimulation:
 
         # get updated obstacles from sumo
         self._current_time_step += 1
-        self._fetch_sumo_vehicles(self.current_time_step)
-        self._fetch_sumo_pedestrians(self.current_time_step)
+        new_vehicle_ids, new_ped_ids = self._init_vehicle_ids()
+        self._fetch_sumo_vehicles(self.current_time_step, new_vehicle_ids)
+        self._fetch_sumo_pedestrians(self.current_time_step, new_ped_ids)
 
     def __presimulation_silent(self, pre_simulation_steps: int, fetch_obstacles=True):
         """
@@ -401,13 +434,14 @@ class SumoSimulation:
         self._silent = True
         for i in range(pre_simulation_steps * self.delta_steps):
             traci.simulationStep()
+            self._init_vehicle_ids()
 
         self._silent = False
         if fetch_obstacles:
-            self._fetch_sumo_vehicles(self.current_time_step)
-            self._fetch_sumo_pedestrians(self.current_time_step)
+            self._fetch_sumo_vehicles(self.current_time_step, self.vehicledomain.getIDList())
+            self._fetch_sumo_pedestrians(self.current_time_step, self.persondomain.getIDList())
 
-    def _fetch_sumo_vehicles(self, time_step: int):
+    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.
 
@@ -425,10 +459,10 @@ class SumoSimulation:
                 continue
 
             # initializes new vehicle
-            if veh_id not in self.ids_sumo2cr[SUMO_VEHICLE_PREFIX]:
+            if veh_id in new_ids:
                 if veh_id.startswith(EGO_ID_START) and not self._silent:
                     # new ego vehicle
-                    cr_id = self._create_cr_id(EGO_ID_START, veh_id, SUMO_VEHICLE_PREFIX)
+                    cr_id = self.ids_sumo2cr[EGO_ID_START][veh_id]
                     if self.dummy_ego_simulation:
                         state.time_step = time_step - 1
                     else:
@@ -445,12 +479,10 @@ class SumoSimulation:
                                                      planning_problem=planning_problem))
                 elif not self._silent:
                     # new obstacle vehicle
-                    cr_id = self._create_cr_id('obstacleVehicle', veh_id, SUMO_VEHICLE_PREFIX)
+                    cr_id = self.ids_sumo2cr['obstacleVehicle'][veh_id]
                     vehicle_class = VEHICLE_TYPE_SUMO2CR[self.vehicledomain.getVehicleClass(str(veh_id))]
-                    shape = self._set_veh_params(self.vehicledomain, veh_id, vehicle_class)
-
                     self.obstacle_types[cr_id] = vehicle_class
-                    self.obstacle_shapes[cr_id] = shape
+                    self.obstacle_shapes[cr_id] = self._get_veh_shape(self.vehicledomain, veh_id)
                     self.obstacle_states[time_step][self.ids_sumo2cr['obstacleVehicle'][veh_id]] = state
             elif veh_id in self.ids_sumo2cr['obstacleVehicle']:
                 # get obstacle vehicle state
@@ -479,7 +511,7 @@ class SumoSimulation:
 
                 ego_veh.set_planned_trajectory(state_list)
 
-    def _fetch_sumo_pedestrians(self, time_step: int):
+    def _fetch_sumo_pedestrians(self, time_step: int, new_ids: List[str]):
         """
         Gets and stores all vehicle states from SUMO. Initializes ego vehicles when they enter simulation.
 
@@ -494,7 +526,7 @@ class SumoSimulation:
             state = self._get_current_state_from_sumo(self.persondomain, str(ped_id), SUMO_PEDESTRIAN_PREFIX)
             if state is None:
                 continue
-            if ped_id not in self.ids_sumo2cr[SUMO_PEDESTRIAN_PREFIX]:
+            if ped_id in new_ids:
                 # initialize new pedestrian
                 cr_id = self._create_cr_id(obstacle_type, ped_id, SUMO_PEDESTRIAN_PREFIX)
                 vehicle_class = VEHICLE_TYPE_SUMO2CR[self.persondomain.getTypeID(ped_id).split("@")[0]]
@@ -507,6 +539,9 @@ class SumoSimulation:
                 # get obstacle vehicle state
                 self.obstacle_states[time_step][self.ids_sumo2cr[obstacle_type][ped_id]] = state
 
+    def _get_veh_shape(self, vehicledomain: VehicleDomain, sumo_id):
+        return Rectangle(vehicledomain.getLength(sumo_id), vehicledomain.getWidth(sumo_id))
+
     def check_ego_collisions(self, raise_error=True):
         """
         Checks if the ego vehicle is colliding in the current time step of the simulation.
@@ -534,7 +569,6 @@ class SumoSimulation:
         :param vehicle_class: vehicle class
         :return:
         """
-
         def sample(val: Union[Interval, float]) -> float:
             if isinstance(val, Interval):
                 assert 0 <= val.start <= val.end, f"All values in the interval need to be positive: {val}"

sumocr/interface/util.py

@@ -12,7 +12,7 @@ import enum
 __author__ = "Moritz Klischat"
 __copyright__ = "TUM Cyber-Physical Systems Group"
 __credits__ = ["ZIM Projekt ZF4086007BZ8"]
-__version__ = "2021.3"
+__version__ = "2021.4"
 __maintainer__ = "Moritz Klischat"
 __email__ = "commonroad@lists.lrz.de"
 __status__ = "Released"

sumocr/maps/scenario_wrapper.py

@@ -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.3"
+__version__ = "2021.4"
 __maintainer__ = "Moritz Klischat"
 __email__ = "commonroad@lists.lrz.de"
 __status__ = "Released"

sumocr/maps/sumo_scenario.py

@@ -26,7 +26,7 @@ except ImportError:
 __author__ = "Moritz Klischat"
 __copyright__ = "TUM Cyber-Physical Systems Group"
 __credits__ = ["ZIM Projekt ZF4086007BZ8"]
-__version__ = "2021.3"
+__version__ = "2021.4"
 __maintainer__ = "Moritz Klischat"
 __email__ = "commonroad@lists.lrz.de"
 __status__ = "Released"

sumocr/maps/util.py

@@ -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.3"
+__version__ = "2021.4"
 __maintainer__ = "Moritz Klischat"
 __email__ = "commonroad@lists.lrz.de"
 __status__ = "Released"

sumocr/visualization/video.py

@@ -86,7 +86,7 @@ def create_video(scenario: Scenario,
 
     dpi = 150
     figsize = (5, 5)
-    rnd = MPRenderer(figsize=figsize)
+    rnd = MPRenderer(figsize=figsize, draw_params={"axis_visible": False})
     rnd.ax.axes.get_xaxis().set_visible(False)
     (ln,) = plt.plot([], [], animated=True)
 
@@ -96,7 +96,7 @@ def create_video(scenario: Scenario,
             planning_problem_set.draw(rnd)
 
         if dynamic_obstacles_ego is not None:
-            rnd.draw_list(dynamic_obstacles_ego, draw_params={'time_begin': 5, 'time_end': 5,
+            rnd.draw_list(dynamic_obstacles_ego, draw_params={'time_begin': 0, 'time_end': 0, "axis_visible": False,
                                           'dynamic_obstacle': {'vehicle_shape': {"occupancy": {"shape": {"rectangle": {
                                               "facecolor": "green"}}}}}})
         scenario.draw(renderer=rnd, draw_params={'time_begin': 0, 'time_end': 0})