Merge branch 'fix-nan' into 'develop'

Fix nan

See merge request cps/commonroad/commonroad-criticality-measures!29

CHANGELOG.md

@@ -5,7 +5,8 @@
 ### Added
 - For evaluating an interval, the starting time step should be always smaller or equal than the ending one.
 ### Fixed
-- for all measures, check whether the time step is valid in the function `validate_update_states_log`. If no, NaN is returned.
+- the memory issues for P_MC measure due to all the simulated vehicle states being stored during the evaluation for subsequent visualization, now the default mode for visualization is off.
+- for all measures, check whether the time step is valid in the function `validate_update_states_log`. If no, `NaN` is returned. Similar to `compute_criticality`: only compute for the valid ego vehicle and other vehicles, otherwise `NaN` is returned
 - check whether the vehicles are in the same lanelet: now the lanelet is extended by its successors and predecessors
 ## [0.3.2 & 0.3.3] - 2024.03.16
 ### Added

commonroad_crime/data_structure/base.py

@@ -13,6 +13,8 @@
 import logging
 from typing import Union
 
+import numpy as np
+
 # CommonRoad packages
 from commonroad.scenario.obstacle import Obstacle, DynamicObstacle, StaticObstacle
 from commonroad.prediction.prediction import TrajectoryPrediction
@@ -247,18 +249,27 @@ def compute_criticality(
         """
         Wrapper for computing the criticality, i.e., the value of the measure.
         """
+        if self.ego_vehicle.state_at_time(time_step) is None:
+            utils_log.print_and_log_warning(
+                logger,
+                f"* The ego vehicle doesn't have state at time step {time_step}",
+            )
+            return np.nan
+
         utils_log.print_and_log_info(
             logger, "*********************************", verbose
         )
 
         self.time_step = time_step
+
         if vehicle_id:
             other_veh_ids = [vehicle_id]
         else:
             other_veh_ids = [
                 veh.obstacle_id
                 for veh in self.sce.obstacles
                 if veh.obstacle_id is not self.ego_vehicle.obstacle_id
+                and veh.state_at_time(self.time_step) is not None
             ]
 
         time_start = time.time()
@@ -280,12 +291,20 @@ def compute_criticality(
                     self.compute(time_step=time_step, vehicle_id=v_id, verbose=verbose)
                 )
             if len([c for c in criti_list if c is not None]) > 0:
+                if np.all(np.isnan(criti_list)):
+                    utils_log.print_and_log_warning(
+                        logger,
+                        f"* Due to the missing entries, all elements are NaN, "
+                        f"the result for time step {time_step} is NaN",
+                    )
+                    return np.nan
+                # Not all elements are NaN, return the max/min of the non-NaN values
                 if self.monotone == TypeMonotone.POS:
-                    criti = max(criti_list)
+                    criti = np.nanmax(criti_list)
                 else:
-                    criti = min(criti_list)
+                    criti = np.nanmin(criti_list)
             else:
-                criti = None
+                return None
         time_computation = time.time() - time_start
         utils_log.print_and_log_info(
             logger, f"*\t\t {self.measure_name} of the scenario: {criti}", verbose

commonroad_crime/data_structure/configuration.py

@@ -374,7 +374,7 @@ class DebugConfiguration(BaseConfig):
     save_plots: bool = True
     plot_limits: Union[List[float], None] = None
     # visualization settings
-    draw_visualization: bool = True
+    draw_visualization: bool = False
     # visualize dynamic obstacles with icons
     draw_icons: bool = True
 

commonroad_crime/measure/probability/p_mc.py

@@ -97,9 +97,11 @@ def compute(self, time_step: int = 0, vehicle_id=None, verbose: bool = True):
             for j in range(len(ego_sl_bundle)):
                 if self.ttc_object.detect_collision(ego_sl_bundle[j]):
                     colliding_prob_list.append(pdf_bundle[j])
-                    self.ego_state_list_set_wc.append(ego_sl_bundle[j])
+                    if self.configuration.debug.draw_visualization:
+                        self.ego_state_list_set_wc.append(ego_sl_bundle[j])
                 else:
-                    self.ego_state_list_set_cf.append(ego_sl_bundle[j])
+                    if self.configuration.debug.draw_visualization:
+                        self.ego_state_list_set_cf.append(ego_sl_bundle[j])
         # (14) in Broadhurst, Adrian, Simon Baker, and Takeo Kanade. "Monte Carlo road safety reasoning." IEEE
         # Proceedings of Intelligent Vehicles Symposium, IEEE, 2005.
         if colliding_prob_list:

tests/test_base.py

@@ -5,7 +5,9 @@
 
 from commonroad_crime.data_structure.scene import Scene
 from commonroad_crime.data_structure.base import CriMeBase
+from commonroad_crime.measure import TTC
 from commonroad_crime.data_structure.configuration import CriMeConfiguration
+from commonroad_crime.data_structure.crime_interface import CriMeInterface
 import commonroad_crime.utility.logger as util_logger
 
 from commonroad_dc.pycrccosy import CurvilinearCoordinateSystem
@@ -52,3 +54,21 @@ def test_clcs(self):
         new_clcs = CurvilinearCoordinateSystem(example_list)
         self.config.update(CLCS=new_clcs)
         self.assertEqual(base.clcs, new_clcs)
+
+    def test_nan_evaluation(self):
+        scenario_id = "USA_US101-5_1_T-1"
+        config = CriMeConfiguration.load(
+            f"../config_files/{scenario_id}.yaml", scenario_id
+        )
+        config.update()
+        config.vehicle.ego_id = 439
+        crime_interface = CriMeInterface(config)
+
+        crime_interface.evaluate_scene([TTC], time_step=30)
+        self.assertEqual(crime_interface.criticality_dict[30][TTC.measure_name], np.inf)
+
+        crime_interface.evaluate_scene([TTC], time_step=35)
+        self.assertTrue(
+            np.isnan(crime_interface.criticality_dict[35][TTC.measure_name]),
+            "The result should be NaN.",
+        )