Merge branch 'bugfix/out-of-projection-domain' into 'develop'

fix logging for out of projection domain

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

CHANGELOG.md

@@ -8,6 +8,7 @@
 - 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
+- adds the error handling of the out-of-projection-domain for many measures, such as TTC, THW, ALongReq, ALatReq
 ## [0.3.2 & 0.3.3] - 2024.03.16
 ### Added
 - Error handling for check in same lanelet

commonroad_crime/measure/acceleration/a_lat_req.py

@@ -97,7 +97,9 @@ def compute(self, vehicle_id: int, time_step: int = 0, verbose: bool = True):
             )[1]
         except ValueError as e:
             utils_log.print_and_log_warning(
-                logger, f"* <A_LAT_REQ> During the projection of the other vehicle: {e}"
+                logger,
+                f"* <A_LAT_REQ> During the projection of the vehicle {self.other_vehicle.obstacle_id} "
+                f"at time step {self.time_step}: {e}",
             )
             # out of projection domain: the other vehicle is far away
             self.value = 0.0

commonroad_crime/measure/acceleration/a_long_req.py

@@ -62,7 +62,8 @@ def compute(self, vehicle_id: int, time_step: int = 0, verbose: bool = True):
         except ValueError as e:
             utils_log.print_and_log_warning(
                 logger,
-                f"* <A_LONG_REQ> During the projection of the other vehicle: {e}",
+                f"* <A_LONG_REQ> During the projection of the vehicle {self.other_vehicle.obstacle_id} "
+                f"at time step {self.time_step}: {e}",
             )
             # out of projection domain: the other vehicle is far away
             a_req = 0.0

commonroad_crime/measure/time/thw.py

@@ -34,15 +34,32 @@ def __init__(self, config: CriMeConfiguration):
         super(THW, self).__init__(config)
 
     def cal_headway(self):
-        other_position = self.other_vehicle.state_at_time(self.time_step).position
-        other_s, _ = self.clcs.convert_to_curvilinear_coords(
-            other_position[0], other_position[1]
-        )
-        ego_position = self.ego_vehicle.state_at_time(self.time_step).position
-        ego_s, _ = self.clcs.convert_to_curvilinear_coords(
-            ego_position[0], ego_position[1]
-        )
-
+        try:
+            other_position = self.other_vehicle.state_at_time(self.time_step).position
+            other_s, _ = self.clcs.convert_to_curvilinear_coords(
+                other_position[0], other_position[1]
+            )
+        except ValueError as e:
+            utils_log.print_and_log_warning(
+                logger,
+                f"* <THW> During the projection of the vehicle {self.other_vehicle.obstacle_id} "
+                f"at time step {self.time_step}: {e}",
+            )
+            # out of projection domain: the other vehicle is far away
+            return math.inf
+        try:
+            ego_position = self.ego_vehicle.state_at_time(self.time_step).position
+            ego_s, _ = self.clcs.convert_to_curvilinear_coords(
+                ego_position[0], ego_position[1]
+            )
+        except ValueError as e:
+            utils_log.print_and_log_warning(
+                logger,
+                f"* <THW> During the projection of the ego vehicle with id {self.ego_vehicle.obstacle_id} "
+                f"at time step {self.time_step}: {e}",
+            )
+            # out of projection domain: the ref path should be problematic
+            return math.nan
         # bump position
         ego_s += self.ego_vehicle.obstacle_shape.length / 2
         other_s -= self.other_vehicle.obstacle_shape.length / 2

commonroad_crime/measure/time/ttc.py

@@ -60,49 +60,69 @@ def compute(self, vehicle_id: int, time_step: int = 0, verbose: bool = True):
             self.value = math.inf
         else:
             # orientation of the ego vehicle and the other vehicle
-            ego_orientation = utils_sol.compute_lanelet_width_orientation(
-                self.sce.lanelet_network.find_lanelet_by_id(lanelet_id[0]),
-                self.ego_vehicle.state_at_time(time_step).position,
-            )[1]
-            other_orientation = utils_sol.compute_lanelet_width_orientation(
-                self.sce.lanelet_network.find_lanelet_by_id(lanelet_id[0]),
-                self.other_vehicle.state_at_time(time_step).position,
-            )[1]
-            # actual velocity and acceleration of both vehicles along the lanelet
-            v_ego = (
-                np.sign(state.velocity)
-                * math.sqrt(state.velocity**2 + state.velocity_y**2)
-                * math.cos(ego_orientation)
-            )
-            # include the directions
-            a_ego = (
-                np.sign(state.acceleration)
-                * math.sqrt(state.acceleration**2 + state.acceleration_y**2)
-                * math.cos(ego_orientation)
-            )
-            if isinstance(self.other_vehicle, DynamicObstacle):
-                v_other = math.sqrt(
-                    state_other.velocity**2 + state_other.velocity_y**2
-                ) * math.cos(other_orientation)
-                a_other = math.sqrt(
-                    state_other.acceleration**2 + state_other.acceleration_y**2
-                ) * math.cos(other_orientation)
+
+            try:
+                ego_orientation = utils_sol.compute_lanelet_width_orientation(
+                    self.sce.lanelet_network.find_lanelet_by_id(lanelet_id[0]),
+                    self.ego_vehicle.state_at_time(time_step).position,
+                )[1]
+            except ValueError as e:
+                utils_log.print_and_log_warning(
+                    logger,
+                    f"* <TTC> During the projection of the ego vehicle {self.ego_vehicle.obstacle_id} "
+                    f"at time step {self.time_step}: {e}",
+                )
+                self.value = math.nan
             else:
-                v_other = 0.0
-                a_other = 0.0
-            delta_v = v_other - v_ego
-            delta_a = a_other - a_ego
-
-            if delta_v < 0 and abs(delta_a) <= 0.1:
-                self.value = utils_gen.int_round(-(delta_d / delta_v), 2)
-            elif delta_v**2 - 2 * delta_d * delta_a < 0:
-                self.value = math.inf
-            elif (delta_v < 0 and delta_a != 0) or (delta_v >= 0 > delta_a):
-                first = -(delta_v / delta_a)
-                second = np.sqrt(delta_v**2 - 2 * delta_d * delta_a) / delta_a
-                self.value = utils_gen.int_round(first - second, 2)
-            else:  # delta_v >= 0 and delta_a >= 0
-                self.value = math.inf
+                try:
+                    other_orientation = utils_sol.compute_lanelet_width_orientation(
+                        self.sce.lanelet_network.find_lanelet_by_id(lanelet_id[0]),
+                        self.other_vehicle.state_at_time(time_step).position,
+                    )[1]
+                except ValueError as e:
+                    utils_log.print_and_log_warning(
+                        logger,
+                        f"* <TTC> During the projection of the vehicle {self.other_vehicle.obstacle_id} "
+                        f"at time step {self.time_step}: {e}",
+                    )
+                    # out of projection domain: the other vehicle is far away
+                    self.value = math.inf
+                else:
+                    # actual velocity and acceleration of both vehicles along the lanelet
+                    v_ego = (
+                        np.sign(state.velocity)
+                        * math.sqrt(state.velocity**2 + state.velocity_y**2)
+                        * math.cos(ego_orientation)
+                    )
+                    # include the directions
+                    a_ego = (
+                        np.sign(state.acceleration)
+                        * math.sqrt(state.acceleration**2 + state.acceleration_y**2)
+                        * math.cos(ego_orientation)
+                    )
+                    if isinstance(self.other_vehicle, DynamicObstacle):
+                        v_other = math.sqrt(
+                            state_other.velocity**2 + state_other.velocity_y**2
+                        ) * math.cos(other_orientation)
+                        a_other = math.sqrt(
+                            state_other.acceleration**2 + state_other.acceleration_y**2
+                        ) * math.cos(other_orientation)
+                    else:
+                        v_other = 0.0
+                        a_other = 0.0
+                    delta_v = v_other - v_ego
+                    delta_a = a_other - a_ego
+
+                    if delta_v < 0 and abs(delta_a) <= 0.1:
+                        self.value = utils_gen.int_round(-(delta_d / delta_v), 2)
+                    elif delta_v**2 - 2 * delta_d * delta_a < 0:
+                        self.value = math.inf
+                    elif (delta_v < 0 and delta_a != 0) or (delta_v >= 0 > delta_a):
+                        first = -(delta_v / delta_a)
+                        second = np.sqrt(delta_v**2 - 2 * delta_d * delta_a) / delta_a
+                        self.value = utils_gen.int_round(first - second, 2)
+                    else:  # delta_v >= 0 and delta_a >= 0
+                        self.value = math.inf
 
         utils_log.print_and_log_info(
             logger, f"*\t\t {self.measure_name} = {self.value}", verbose