解决一些bug，安全性航向角问题；舒适性画龙识别阈值；高效性阈值与高效性指标m./s转换成km/h；交规性指标中车辆速度v，因为没有深度拷贝导致多乘3.6。更新yaml指标配置文件，注销掉骑行线指标，更改安全性MTTC，高效性一些指标阈值

config/metric_config.yaml

@@ -1,20 +1,23 @@
 vehicle:
-  CAR_WIDTH: 1.872
-  CAR_LENGTH: 4.924
-  CAR_HEIGHT: 1.3
-  CAR_OFFX: 1.321
-  RHO: 0.3
-  EGO_ACCEL_MAX: 6
-  OBJ_DECEL_MAX: 8
-  EGO_DECEL_MIN: 1
-  EGO_DECEL_LON_MAX: 8
-  EGO_DECEL_LAT_MAX: 1
-  EGO_WHEELBASS: 2.8
+  # 车辆参数配置
+  CAR_WIDTH: 2.065       # 车体宽度（单位：米）
+  CAR_LENGTH: 5.990      # 车体长度（单位：米）
+  CAR_HEIGHT: 2.820        # 车体高度（单位：米）新增
+  CAR_OFFX: 1.321        # 车辆长度方向参考点到几何中心点的距离（单位：米）新增
+  RHO: 0.3               # 驾驶员制动反应时间（单位：秒）
+  EGO_ACCEL_MAX: 2.268       # 自车油门最大加速度（单位：m/s²）
+  OBJ_DECEL_MAX: 8       # 前车刹车最大减速度（单位：m/s²）
+  EGO_DECEL_MIN: 0.01       # 自车刹车最小减速度（需确认单位：m/s²）
+  EGO_DECEL_LON_MAX: 5.7869   # 自车纵向刹车最大减速度（单位：m/s²）
+  EGO_DECEL_LAT_MAX: 0   # 自车横向刹车最大减速度（单位：m/s²）
+  EGO_WHEELBASS: 3.8     # 自车轮距（单位：米）
 
 T_threshold:
-  T0_threshold: 0
-  T1_threshold: 2
-  T2_threshold: 5
+  T0_threshold: 0  # 表示T0等级的通过阈值
+  T1_threshold: 2  # 表示T1等级的通过阈值
+  T2_threshold: 5  # 表示T2等级的通过阈值
+
+
 
 safety:
   name: safety
@@ -31,7 +34,7 @@ safety:
       name: MTTC
       priority: 0
       max: 2000.0
-      min: 3.0
+      min: 0.8
     THW:
       name: THW
       priority: 0
@@ -40,24 +43,34 @@ safety:
   safeDistance:
     name: safeDistance
     priority: 0
-    LonSD:
-      name: LonSD
-      priority: 0
-      max: 2000.0
-      min: 10.0
+    # LonSD:
+    #   name: LonSD
+    #   priority: 0
+    #   max: 2000.0
+    #   min: 10.0
     LatSD:
-      name: LatSD
+      name: LatSD 
       priority: 0
       max: 2000.0
       min: 2.0
   safeAcceleration:
     name: safeAcceleration
     priority: 0
+    # DRAC:
+    #   name: DRAC
+    #   priority: 0
+    #   max: 5.0
+    #   min: -2000.0
     BTN:
       name: BTN
       priority: 0
       max: 1.0
       min: -2000.0
+    # STN:
+    #   name: STN
+    #   priority: 0
+    #   max: 1.0
+    #   min: -2000.0
   safeProbability:
     name: safeProbability
     priority: 0
@@ -71,7 +84,6 @@ safety:
       priority: 0
       max: 10.0
       min: 0.0
-
 comfort:
   name: comfort
   priority: 0
@@ -97,7 +109,7 @@ comfort:
       max: 0
       min: 0
     slamBrake:
-      name: slamBrake
+      name: slamBrake 
       priority: 0
       max: 0
       min: 0
@@ -106,7 +118,6 @@ comfort:
       priority: 0
       max: 0
       min: 0
-
 efficient:
   name: efficient
   priority: 0
@@ -116,12 +127,12 @@ efficient:
     max_speed:
       name: maxSpeed
       priority: 0
-      max: 0.0
-      min: 0.0
+      max: 80.0
+      min: 30.0
     devation_speed:
       name: deviationSpeed
       priority: 0
-      max: 0.0
+      max: 20.0
       min: 0.0
     averagedSpeed:
       name: averagedSpeed
@@ -136,7 +147,6 @@ efficient:
       priority: 0
       max: 1
       min: 0
-
 function:
   name: function
   priority: 0
@@ -163,190 +173,245 @@ function:
       priority: 0
       max: 17.29
       min: 10.51
-
+    rightWarningSignal_LST:
+      name: rightWarningSignal_LST
+      priority: 0
+      max: 1
+      min: 0
 traffic:
   name: traffic
   priority: 0
+  #重大违规 12分
   majorViolation:
     name: majorViolation
     priority: 0
+    #urbanExpresswayOrHighwaySpeedOverLimit50：表示在高速公路或城市快速路上，机动车驾驶人超速50%以上的；
     urbanExpresswayOrHighwaySpeedOverLimit50:
       name: urbanExpresswayOrHighwaySpeedOverLimit50
       priority: 0
       max: 0
       min: 0
+    #urbanExpresswayOrHighwayReverse：表示在高速公路或城市快速路上，机动车倒车行驶；
+    urbanExpresswayOrHighwayReverse:
+      name:
+      priority: 0
+      max: 0
+      min: 0
+    #urbanExpresswayOrHighwayDrivingAgainst：表示在高速公路或城市快速路上，机动车逆行；
+    urbanExpresswayOrHighwayDrivingAgainst:
+      name: higwayDrivingAgainst
+      priority: 0
+      max: 0
+      min: 0
 
+  #严重违规 9分
   seriousViolation:
     name: seriousViolation
     priority: 0
+    #urbanExpresswayOrHighwayDrivingLaneStopped：表示在高速公路或城市快速路上，机动车行驶车道停车；
     urbanExpresswayOrHighwayDrivingLaneStopped:
       name: urbanExpresswayOrHighwayDrivingLaneStopped
       priority: 0
       max: 0
       min: 0
+    #urbanExpresswayOrHighwayEmergencyLaneStopped：表示在高速公路或城市快速路上，机动车应急车道内停车的；
     urbanExpresswayOrHighwayEmergencyLaneStopped:
-      name: urbanExpresswayOrHighwayEmergencyLaneStopped
+      name: highwayEmergencyLaneStopped
       priority: 0
       max: 0
       min: 0
 
+  #危险违规 6分
   dangerousViolation:
     name: dangerousViolation
     priority: 0
+    #urbanExpresswayEmergencyLaneDriving：非紧急情况时在城市快速路或者高速公路应急车道上行驶的；
     urbanExpresswayEmergencyLaneDriving:
       name: urbanExpresswayEmergencyLaneDriving
       priority: 0
       max: 0
       min: 0
+    #trafficSignalViolation：表示在交通信号灯控制的道路上，机动车驾驶人违反规定的；
     trafficSignalViolation:
       name: trafficSignalViolation
       priority: 0
       max: 0
       min: 0
+    #urbanExpresswayOrHighwaySpeedOverLimit20to50：表示在高速公路或城市快速路上，机动车驾驶人超速20%-50%的；
     urbanExpresswayOrHighwaySpeedOverLimit20to50:
       name: urbanExpresswayOrHighwaySpeedOverLimit20to50
       priority: 0
       max: 0
       min: 0
+    #generalRoadSpeedOverLimit50：表示在非高速公路或城市快速路，机动车驾驶人超速50%的；
     generalRoadSpeedOverLimit50:
       name: generalRoadSpeedOverLimit50
       priority: 0
       max: 0
       min: 0
 
+  #一般违规 3分
   generalViolation:
     name: generalViolation
     priority: 0
+    #generalRoadSpeedOverLimit20to50：表示在非高速公路或城市快速路，机动车驾驶人超速20%到50%的；
     generalRoadSpeedOverLimit20to50:
       name: generalRoadSpeedOverLimit20to50
       priority: 0
       max: 0
       min: 0
+    #urbanExpresswayOrHighwaySpeedUnderLimit: 对于驾驶机动车在高速公路或城市快速路上行驶低于规定最低时速的,将处以记3分的处罚；
     urbanExpresswayOrHighwaySpeedUnderLimit:
       name: UrbanExpresswayOrHighwaySpeedUnderLimit
       priority: 0
       max: 0
       min: 0
+    #illegalDrivingOrParkingAtCrossroads：行经交叉路口不按规定行车或者停车，通过路口遇停止信号时，停在停止线以内或路口内的；
     illegalDrivingOrParkingAtCrossroads:
       name: illegalDrivingOrParkingAtCrossroads
       priority: 0
       max: 0
       min: 0
+    # 超车类型
+    # overtake_on_right: 从前车右侧超车的
     overtake_on_right:
       name: overtake_on_right
       priority: 0
       max: 0
       min: 0
+    # overtake_when_turn_around: 前车掉头时超车的
     overtake_when_turn_around:
       name: overtake_when_turn_around
       priority: 0
       max: 0
       min: 0
+    # overtake_when_passing_car: 前车通过时超车的
     overtake_when_passing_car:
       name: overtake_when_passing_car
       priority: 0
       max: 0
       min: 0
+    # overtake_in_forbid_lane: 在不该占用车道超车的，前方车辆缓慢行驶
     overtake_in_forbid_lane:
       name: overtake_in_forbid_lane
       priority: 0
       max: 0
       min: 0
+    # overtake_in_ramp: 在匝道超车的
     overtake_in_ramp:
       name: overtake_in_ramp
       priority: 0
       max: 0
       min: 0
+    # overtake_in_tunnel: 在隧道超车的
     overtake_in_tunnel:
       name: overtake_in_tunnel
       priority: 0
       max: 0
       min: 0
+    # overtake_on_accelerate_lane: 在加速车道超车
     overtake_on_accelerate_lane:
       name: overtake_on_accelerate_lane
       priority: 0
       max: 0
       min: 0
+    # overtake_on_decelerate_lane: 在减速车道超车
     overtake_on_decelerate_lane:
       name: overtake_on_decelerate_lane
       priority: 0
       max: 0
       min: 0
+    # overtake_in_different_senerios: 在不同路口超车情况超车的
     overtake_in_different_senerios:
       name: overtake_in_different_senerios
       priority: 0
       max: 0
       min: 0
+    # 减速让行违规类
+    # slow_down_in_crosswalk: 行经人行横道，未减速行驶的
     slow_down_in_crosswalk:
       name: slow_down_in_crosswalk
       priority: 0
       max: 0
       min: 0
+    # avoid_pedestrian_in_crosswalk：遇行人正在通过人行横道时未停车让行的
     avoid_pedestrian_in_crosswalk:
       name: avoid_pedestrian_in_crosswalk
       priority: 0
       max: 0
       min: 0
+    # avoid_pedestrian_in_the_road :行经没有交通信号的道路时，遇行人横过道路未避让的
     avoid_pedestrian_in_the_road:
       name: avoid_pedestrian_in_the_road
       priority: 0
       max: 0
       min: 0
+    # aviod_pedestrian_when_turning:转弯的机动车未让行人先行的
     aviod_pedestrian_when_turning:
       name: aviod_pedestrian_when_turning
       priority: 0
       max: 0
       min: 0
+
+    # 违反交通标志
+    # NoStraightThrough: 禁止直行标志地方直行
     NoStraightThrough:
       name: NoStraightThrough
       priority: 0
       max: 0
       min: 0
+    
+    # SpeedLimitViolation: 违反限速规定
     SpeedLimitViolation:
       name: SpeedLimitViolation
       priority: 0
       max: 0
       min: 0
+
+    # MinimumSpeedLimitViolation: 违反最低限速规定
     MinimumSpeedLimitViolation:
       name: MinimumSpeedLimitViolation
       priority: 0
       max: 0
       min: 0
-
+  #轻度违规 1分
   minorViolation:
     name: minorViolation
     priority: 0
-    turn_back_in_forbiden_turn_back_sign:
-      name: turn_back_in_forbiden_turn_back_sign
-      priority: 0
-      max: 0
-      min: 0
-    turn_back_in_forbiden_turn_left_sign:
-      name: turn_back_in_forbiden_turn_left_sign
-      priority: 0
-      max: 0
-      min: 0
-    avoid_pedestrian_when_turn_back:
-      name: avoid_pedestrian_when_turn_back
+    noUTurnViolation:
+      #NoU：是“No U-Turn”（禁止掉头）的缩写，表示这一行为是违反禁止掉头规定的。
+      #TurnViolation：表示这是一种违规行为，即掉头行为本身是不被允许的。
+      name: noUTurnViolation
       priority: 0
       max: 0
       min: 0
 
+
+  #警告违规 0分
   warningViolation:
     name: warningViolation
     priority: 0
+    # generalLimit60RoadSpeedOverLimit0to50：驾驶机动车在限速低于60公里/小时的公路上超过规定车速50%以下的，该指标暂时不做
+    # generalLimit60RoadSpeedOverLimit0to50:
+    #   name: generalRoadSpeedOverLimit20to50
+    #   priority: 0
+    #   max: 0
+    #   min: 0
+    # urbanExpresswayOrHighwaySpeedOverLimit0to20：在高速公路或城市快速路，驾驶机动车超速20%以下的
     urbanExpresswayOrHighwaySpeedOverLimit0to20:
       name: urbanExpresswayOrHighwaySpeedOverLimit0to20
       priority: 0
       max: 0
       min: 0
-    urbanExpresswayOrHighwayRideLaneDivider:
-      name: urbanExpresswayOrHighwayRideLaneDivider
-      priority: 0
-      max: 0
-      min: 0
+    # urbanExpresswayOrHighwayRideLaneDivider： 机动车在高速公路或者城市快速路上骑轧车行道分界线的
+    # urbanExpresswayOrHighwayRideLaneDivider:
+    #   name: urbanExpresswayOrHighwayRideLaneDivider
+    #   priority: 0
+    #   max: 0
+    #   min: 0
+    # generalRoadIrregularLaneUse：驾驶机动车在高速公路、城市快速路以外的道路上不按规定车道行驶的,这里指的是车辆占用非机动车道；
     generalRoadIrregularLaneUse:
       name: generalRoadIrregularLaneUse
       priority: 0
       max: 0
-      min: 0
+      min: 0

modules/metric/comfort.py

@@ -1,29 +1,13 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
-##################################################################
-#
-# Copyright (c) 2023 CICV, Inc. All Rights Reserved
-#
-##################################################################
-"""
-@Authors:           zhanghaiwen(zhanghaiwen@china-icv.cn), yangzihao(yangzihao@china-icv.cn)
-@Data:              2023/06/25
-@Last Modified:     2023/06/25
-@Summary:           Comfort metrics
-"""
-
-import sys
+
 import math
 import pandas as pd
 import numpy as np
 import scipy.signal
-from pathlib import Path 
-
 from modules.lib.score import Score
 from modules.lib.common import get_interpolation, get_frame_with_time
 from modules.config import config
-from modules.lib import data_process
-
 from modules.lib.log_manager import LogManager
 
 
@@ -39,7 +23,6 @@ def peak_valley_decorator(method):
                 p_curr = pv_list[i]
 
                 if self._peak_valley_judgment(p_last, p_curr):
-                    # method(self, p_curr, p_last)
                     method(self, p_curr, p_last, flag, *args, **kwargs)
                 else:
                     p_last = p_curr
@@ -64,60 +47,32 @@ class Comfort(object):
     """
 
     def __init__(self, data_processed):
-
-        # self.logger = log.get_logger()
-        self.eval_data = pd.DataFrame()
         self.data_processed = data_processed
-        self.logger = LogManager().get_logger()  # 获取全局日志实例
+        self.logger = LogManager().get_logger()
 
-        self.data = data_processed.ego_data
-        # self.mileage = data_processed.report_info['mileage']
+        self.data = data_processed.ego_data.copy()
         self.ego_df = pd.DataFrame()
         self.discomfort_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
 
-        
         self.calculated_value = {  
-            'Weaving': 0,  
+            'weaving': 0,  
             'shake': 0,  
             'cadence': 0,  
             'slamBrake': 0,  
             'slamAccelerate': 0,   
         } 
 
-
-
-        # self.time_list = data_processed.driver_ctrl_data['time_list']
-        # self.frame_list = data_processed.driver_ctrl_data['frame_list']
-
         self.time_list = self.data['simTime'].values.tolist()
         self.frame_list = self.data['simFrame'].values.tolist()
 
-        self.count_dict = {}
-        self.duration_dict = {}
-        self.strength_dict = {}
-
-        self.discomfort_count = 0
+        # 移除未使用的字典
         self.zigzag_count = 0
         self.shake_count = 0
         self.cadence_count = 0
         self.slam_brake_count = 0
         self.slam_accel_count = 0
 
-        self.zigzag_strength = 0
-        self.shake_strength = 0
-        self.cadence_strength = 0
-        self.slam_brake_strength = 0
-        self.slam_accel_strength = 0
-
-        self.discomfort_duration = 0
-        self.zigzag_duration = 0
-        self.shake_duration = 0
-        self.cadence_duration = 0
-        self.slam_brake_duration = 0
-        self.slam_accel_duration = 0
-
         self.zigzag_time_list = []
-        self.zigzag_frame_list = []
         self.zigzag_stre_list = []
         self.cur_ego_path_list = []
         self.curvature_list = []
@@ -126,25 +81,12 @@ class Comfort(object):
         self._comf_param_cal()
 
     def _get_data(self):
-        """
-
-        """
+        """获取舒适性评估所需数据"""
         self.ego_df = self.data[config.COMFORT_INFO].copy()
-        self.df = self.ego_df.reset_index(drop=True)  # 索引是csv原索引
-
-    # def _cal_cur_ego_path(self, row):
-    #     try:
-    #         divide = (row['speedX'] ** 2 + row['speedY'] ** 2) ** (3 / 2)
-    #         if not divide:
-    #             res = None
-    #         else:
-    #             res = (row['speedX'] * row['accelY'] - row['speedY'] * row['accelX']) / divide
-    #     except:
-    #         res = None
-    #     return res
-    import numpy as np
+        self.df = self.ego_df.reset_index(drop=True)
 
     def _cal_cur_ego_path(self, row):
+        """计算车辆轨迹曲率"""
         try:
             # 计算速度平方和，判断是否接近零
             speed_sq = row['speedX']**2 + row['speedY']**2
@@ -153,16 +95,12 @@ class Comfort(object):
             divide = speed_sq ** (3/2)
             res = (row['speedX'] * row['accelY'] - row['speedY'] * row['accelX']) / divide
             return res
-        except Exception as e:
+        except Exception:
             return 1e5  # 异常时也返回极大值（如除零、缺失值等）
 
-
     def _comf_param_cal(self):
-        """
-
-        """
-
-        # [log]
+        """计算舒适性相关参数"""
+        # 加减速阈值计算
         self.ego_df['ip_acc'] = self.ego_df['v'].apply(get_interpolation, point1=[18, 4], point2=[72, 2])
         self.ego_df['ip_dec'] = self.ego_df['v'].apply(get_interpolation, point1=[18, -5], point2=[72, -3.5])
         self.ego_df['slam_brake'] = (self.ego_df['lon_acc'] - self.ego_df['ip_dec']).apply(
@@ -172,7 +110,7 @@ class Comfort(object):
         self.ego_df['cadence'] = self.ego_df.apply(
             lambda row: self._cadence_process_new(row['lon_acc'], row['ip_acc'], row['ip_dec']), axis=1)
 
-        # for shake detector
+        # 晃动检测相关参数
         self.ego_df['cur_ego_path'] = self.ego_df.apply(self._cal_cur_ego_path, axis=1)
         self.ego_df['curvHor'] = self.ego_df['curvHor'].astype('float')
         self.ego_df['cur_diff'] = (self.ego_df['cur_ego_path'] - self.ego_df['curvHor']).abs()
@@ -185,33 +123,17 @@ class Comfort(object):
 
     def _peak_valley_determination(self, df):
         """
-        Determine the peak and valley of the vehicle based on its current angular velocity.
-
-        Parameters:
-            df: Dataframe containing the vehicle angular velocity.
-
-        Returns:
-            peak_valley: List of indices representing peaks and valleys.
+        确定车辆角速度的峰值和谷值
         """
-
-        peaks, _ = scipy.signal.find_peaks(df['speedH'], height=0.01, distance=1, prominence=0.01)
-        valleys, _ = scipy.signal.find_peaks(-df['speedH'], height=0.01, distance=1, prominence=0.01)
+        # 调整参数以减少噪音干扰
+        peaks, _ = scipy.signal.find_peaks(df['speedH'], height=0.03, distance=3, prominence=0.03, width=1)
+        valleys, _ = scipy.signal.find_peaks(-df['speedH'], height=0.03, distance=3, prominence=0.03, width=1)
         peak_valley = sorted(list(peaks) + list(valleys))
-
         return peak_valley
 
-    def _peak_valley_judgment(self, p_last, p_curr, tw=10000, avg=0.02):
+    def _peak_valley_judgment(self, p_last, p_curr, tw=100, avg=0.06):
         """
-        Determine if the given peaks and valleys satisfy certain conditions.
-
-        Parameters:
-            p_last: Previous peak or valley data point.
-            p_curr: Current peak or valley data point.
-            tw: Threshold time difference between peaks and valleys.
-            avg: Angular velocity gap threshold.
-
-        Returns:
-            Boolean indicating whether the conditions are satisfied.
+        判断给定的峰值和谷值是否满足特定条件
         """
         t_diff = p_curr[0] - p_last[0]
         v_diff = abs(p_curr[1] - p_last[1])
@@ -224,29 +146,13 @@ class Comfort(object):
 
     @peak_valley_decorator
     def zigzag_count_func(self, p_curr, p_last, flag=True):
-        """
-        Count the number of zigzag movements.
-
-        Parameters:
-            df: Input dataframe data.
-
-        Returns:
-            zigzag_count: Number of zigzag movements.
-        """
+        """计算曲折行驶次数"""
         if flag:
             self.zigzag_count += 1
-        else:
-            self.zigzag_count += 0
 
     @peak_valley_decorator
     def cal_zigzag_strength_strength(self, p_curr, p_last, flag=True):
-        """
-        Calculate various strength statistics.
-
-        Returns:
-            Tuple containing maximum strength, minimum strength,
-            average strength, and 99th percentile strength.
-        """
+        """计算曲折行驶强度"""
         if flag:
             v_diff = abs(p_curr[1] - p_last[1])
             t_diff = p_curr[0] - p_last[0]
@@ -256,23 +162,14 @@ class Comfort(object):
             self.zigzag_stre_list = []
 
     def _shake_detector(self, Cr_diff=0.05, T_diff=0.39):
-        """
-        ego车横向加速度ax;
-        ego车轨迹横向曲率;
-        ego车轨迹曲率变化率;
-        ego车所在车lane曲率;
-        ego车所在车lane曲率变化率;
-        转向灯（暂时存疑，可不用）Cr_diff = 0.1, T_diff = 0.04
-        求解曲率公式k(t) = (x'(t) * y''(t) - y'(t) * x''(t)) / ((x'(t))^2 + (y'(t))^2)^(3/2)
-        """
+        """检测晃动事件"""
         time_list = []
         frame_list = []
-        shake_time_list = []
 
         df = self.ego_df.copy()
         df = df[df['cur_diff'] > Cr_diff]
         df['frame_ID_diff'] = df['simFrame'].diff()  # 找出行车轨迹曲率与道路曲率之差大于阈值的数据段
-        filtered_df = df[df.frame_ID_diff > T_diff]  # 此处是用大间隔区分多次晃动情景    。
+        filtered_df = df[df.frame_ID_diff > T_diff]  # 此处是用大间隔区分多次晃动情景
 
         row_numbers = filtered_df.index.tolist()
         cut_column = pd.cut(df.index, bins=row_numbers)
@@ -287,8 +184,6 @@ class Comfort(object):
             df_group['curvHor'] = df_group['curvHor'].abs()
             df_group_straight = df_group[(df_group.lightMask == 0) & (df_group.curvHor < 0.001)]
             if not df_group_straight.empty:
-                tmp_list = df_group_straight['simTime'].values
-                # shake_time_list.append([tmp_list[0], tmp_list[-1]])
                 time_list.extend(df_group_straight['simTime'].values)
                 frame_list.extend(df_group_straight['simFrame'].values)
                 self.shake_count = self.shake_count + 1
@@ -297,8 +192,6 @@ class Comfort(object):
             df_group_change_lane = df_group[(df_group['lightMask'] != 0) & (df_group['curvHor'] < 0.001)]
             df_group_change_lane_data = df_group_change_lane[df_group_change_lane.cur_diff > Cr_diff + 0.2]
             if not df_group_change_lane_data.empty:
-                tmp_list = df_group_change_lane_data['simTime'].values
-                # shake_time_list.append([tmp_list[0], tmp_list[-1]])
                 time_list.extend(df_group_change_lane_data['simTime'].values)
                 frame_list.extend(df_group_change_lane_data['simFrame'].values)
                 self.shake_count = self.shake_count + 1
@@ -307,8 +200,6 @@ class Comfort(object):
             df_group_turn = df_group[(df_group['lightMask'] != 0) & (df_group['curvHor'].abs() > 0.001)]
             df_group_turn_data = df_group_turn[df_group_turn.cur_diff.abs() > Cr_diff + 0.1]
             if not df_group_turn_data.empty:
-                tmp_list = df_group_turn_data['simTime'].values
-                # shake_time_list.append([tmp_list[0], tmp_list[-1]])
                 time_list.extend(df_group_turn_data['simTime'].values)
                 frame_list.extend(df_group_turn_data['simFrame'].values)
                 self.shake_count = self.shake_count + 1
@@ -330,13 +221,13 @@ class Comfort(object):
                 sub_group_time = [t_list[i]]
                 sub_group_frame = [f_list[i]]
 
-
         # 输出图表值
         shake_time = [[g[0], g[-1]] for g in group_time]
         shake_frame = [[g[0], g[-1]] for g in group_frame]
         self.shake_count = len(shake_time)
 
         if shake_time:
+            # 保存晃动事件摘要
             time_df = pd.DataFrame(shake_time, columns=['start_time', 'end_time'])
             frame_df = pd.DataFrame(shake_frame, columns=['start_frame', 'end_frame'])
             discomfort_df = pd.concat([time_df, frame_df], axis=1)
@@ -345,21 +236,10 @@ class Comfort(object):
 
         return time_list
 
-    def _cadence_process(self, lon_acc_roc, ip_dec_roc):
-        if abs(lon_acc_roc) >= abs(ip_dec_roc) or abs(lon_acc_roc) < 1:
-            return np.nan
-        # elif abs(lon_acc_roc) == 0:
-        elif abs(lon_acc_roc) == 0:
-            return 0
-        elif lon_acc_roc > 0 and lon_acc_roc < -ip_dec_roc:
-            return 1
-        elif lon_acc_roc < 0 and lon_acc_roc > ip_dec_roc:
-            return -1
-
     def _cadence_process_new(self, lon_acc, ip_acc, ip_dec):
+        """处理顿挫数据"""
         if abs(lon_acc) < 1 or lon_acc > ip_acc or lon_acc < ip_dec:
             return np.nan
-        # elif abs(lon_acc_roc) == 0:
         elif abs(lon_acc) == 0:
             return 0
         elif lon_acc > 0 and lon_acc < ip_acc:
@@ -370,17 +250,8 @@ class Comfort(object):
             return 0
 
     def _cadence_detector(self):
-        """
-        # 加速度突变：先加后减，先减后加，先加然后停，先减然后停
-        # 顿挫：2s内多次加速度变化率突变
-        # 求出每一个特征点，然后提取，然后将每一个特征点后面的2s做一个窗口，统计频率，避免无效运算
-
-        # 将特征点筛选出来
-        # 将特征点时间作为聚类标准，大于1s的pass，小于等于1s的聚类到一个分组
-        # 去掉小于3个特征点的分组
-        """
-        # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'cadence']].copy()
-        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'cadence']].copy()
+        """检测顿挫事件"""
+        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'cadence', 'v']].copy()
         time_list = data['simTime'].values.tolist()
 
         data = data[data['cadence'] != np.nan]
@@ -411,14 +282,12 @@ class Comfort(object):
         group_time = [g for g in group_time if len(g) >= 1]  # 有一次特征点则算作一次顿挫
         group_frame = [g for g in group_frame if len(g) >= 1]
 
-
-        # 将顿挫组的起始时间为组重新统计时间
-
         # 输出图表值
         cadence_time = [[g[0], g[-1]] for g in group_time]
         cadence_frame = [[g[0], g[-1]] for g in group_frame]
 
         if cadence_time:
+            # 保存顿挫事件摘要
             time_df = pd.DataFrame(cadence_time, columns=['start_time', 'end_time'])
             frame_df = pd.DataFrame(cadence_frame, columns=['start_frame', 'end_frame'])
             discomfort_df = pd.concat([time_df, frame_df], axis=1)
@@ -426,40 +295,16 @@ class Comfort(object):
             self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)
 
         # 将顿挫组的起始时间为组重新统计时间
-        cadence_time_list = [time for pair in cadence_time for time in time_list if pair[0] <= time <= pair[1]]
-
-        # time_list = [element for sublist in group_time for element in sublist]
-        # merged_list = [element for sublist in res_group for element in sublist]
-        # res_df = data[data['simTime'].isin(merged_list)]
-
-        stre_list = []
-        freq_list = []
-        for g in group_time:
-            # calculate strength
-            g_df = data[data['simTime'].isin(g)]
-            strength = g_df['lon_acc'].abs().mean()
-            stre_list.append(strength)
-
-            # calculate frequency
-            cnt = len(g)
-            t_start = g_df['simTime'].iloc[0]
-            t_end = g_df['simTime'].iloc[-1]
-            t_delta = t_end - t_start
-            frequency = cnt / t_delta
-            freq_list.append(frequency)
-
-        self.cadence_count = len(freq_list)
-        cadence_stre = sum(stre_list) / len(stre_list) if stre_list else 0
+        cadence_time_list = [time for pair in cadence_time for time in self.ego_df['simTime'].values if pair[0] <= time <= pair[1]]
+
+        self.cadence_count = len(cadence_time)
 
         return cadence_time_list
 
     def _slam_brake_detector(self):
-        # 统计急刹全为1的分段的个数，记录分段开头的frame_ID
-        # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'ip_dec_roc', 'slam_brake']].copy()
-        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'ip_dec', 'slam_brake']].copy()
-        # data['slam_diff'] = data['slam_brake'].diff()
-        # res_df = data[data['slam_diff'] == 1]
-
+        """检测急刹车事件"""
+        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'ip_dec', 'slam_brake', 'v']].copy()
+        
         res_df = data[data['slam_brake'] == 1]
         t_list = res_df['simTime'].values
         f_list = res_df['simFrame'].values.tolist()
@@ -489,6 +334,7 @@ class Comfort(object):
         slam_brake_frame = [[g[0], g[-1]] for g in group_frame]
 
         if slam_brake_time:
+            # 保存事件摘要
             time_df = pd.DataFrame(slam_brake_time, columns=['start_time', 'end_time'])
             frame_df = pd.DataFrame(slam_brake_frame, columns=['start_frame', 'end_frame'])
             discomfort_df = pd.concat([time_df, frame_df], axis=1)
@@ -496,16 +342,13 @@ class Comfort(object):
             self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)
 
         time_list = [element for sublist in group_time for element in sublist]
-        self.slam_brake_count = len(group_time)  # / self.mileage  # * 1000000
+        self.slam_brake_count = len(group_time)
         return time_list
 
     def _slam_accel_detector(self):
-        # 统计急刹全为1的分段的个数，记录分段开头的frame_ID
-        # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'ip_acc_roc', 'slam_accel']].copy()
-        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'ip_acc', 'slam_accel']].copy()
-        # data['slam_diff'] = data['slam_accel'].diff()
-        # res_df = data.loc[data['slam_diff'] == 1]
-
+        """检测急加速事件"""
+        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'ip_acc', 'slam_accel', 'v']].copy()
+        
         res_df = data.loc[data['slam_accel'] == 1]
         t_list = res_df['simTime'].values
         f_list = res_df['simFrame'].values.tolist()
@@ -529,12 +372,12 @@ class Comfort(object):
         group_time = [g for g in group_time if len(g) >= 2]
         group_frame = [g for g in group_frame if len(g) >= 2]
 
-
         # 输出图表值
         slam_accel_time = [[g[0], g[-1]] for g in group_time]
         slam_accel_frame = [[g[0], g[-1]] for g in group_frame]
 
         if slam_accel_time:
+            # 保存事件摘要
             time_df = pd.DataFrame(slam_accel_time, columns=['start_time', 'end_time'])
             frame_df = pd.DataFrame(slam_accel_frame, columns=['start_frame', 'end_frame'])
             discomfort_df = pd.concat([time_df, frame_df], axis=1)
@@ -542,22 +385,21 @@ class Comfort(object):
             self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)
 
         time_list = [element for sublist in group_time for element in sublist]
-        self.slam_accel_count = len(group_time)  # / self.mileage  # * 1000000
+        self.slam_accel_count = len(group_time)
         return time_list
-
+    
     def comf_statistic(self):
-        
-        df = self.ego_df[['simTime', 'cur_diff', 'lon_acc', 'lon_acc_roc', 'accelH']].copy()
+        """统计舒适性指标"""
+        df = self.ego_df[['simTime', 'simFrame', 'cur_diff', 'lon_acc', 'lon_acc_roc', 'accelH', 'speedH', 'lat_acc', 'v']].copy()
 
         self.zigzag_count_func()
         self.cal_zigzag_strength_strength()
         if self.zigzag_time_list:
+            # 保存 Weaving (zigzag) 事件摘要
             zigzag_df = pd.DataFrame(self.zigzag_time_list, columns=['start_time', 'end_time'])
             zigzag_df = get_frame_with_time(zigzag_df, self.ego_df)
             zigzag_df['type'] = 'zigzag'
             self.discomfort_df = pd.concat([self.discomfort_df, zigzag_df], ignore_index=True)
-            # discomfort_df = pd.concat([time_df, frame_df], axis=1)
-            # self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)
 
         zigzag_t_list = []
         # 只有[t_start, t_end]数对，要提取为完整time list
@@ -572,25 +414,7 @@ class Comfort(object):
         slam_brake_t_list = self._slam_brake_detector()
         slam_accel_t_list = self._slam_accel_detector()
 
-    
-
-        discomfort_time_list = zigzag_t_list + shake_t_list + cadence_t_list + slam_brake_t_list + slam_accel_t_list
-        discomfort_time_list = sorted(discomfort_time_list)  # 排序
-        discomfort_time_list = list(set(discomfort_time_list))  # 去重
-
-        # TIME_DIFF = self.time_list[3] - self.time_list[2]
-        # TIME_DIFF = 0.4
-        FREQUENCY = 100
-        TIME_DIFF = 1 / FREQUENCY
-        self.discomfort_duration = len(discomfort_time_list) * TIME_DIFF
-
-        df['flag_zigzag'] = df['simTime'].apply(lambda x: 1 if x in zigzag_t_list else 0)
-        df['flag_shake'] = df['simTime'].apply(lambda x: 1 if x in shake_t_list else 0)
-        df['flag_cadence'] = df['simTime'].apply(lambda x: 1 if x in cadence_t_list else 0)
-        df['flag_slam_brake'] = df['simTime'].apply(lambda x: 1 if x in slam_brake_t_list else 0)
-        df['flag_slam_accel'] = df['simTime'].apply(lambda x: 1 if x in slam_accel_t_list else 0)
-
-
+        # 统计结果
         self.calculated_value = {
             "weaving": self.zigzag_count,
             "shake": self.shake_count,
@@ -601,38 +425,10 @@ class Comfort(object):
         self.logger.info(f"舒适性计算完成，统计结果：{self.calculated_value}")
         return self.calculated_value
 
-    def _nan_detect(self, num):
-        if math.isnan(num):
-            return 0
-        return num
-
-    def zip_time_pairs(self, zip_list):
-        zip_time_pairs = zip(self.time_list, zip_list)
-        zip_vs_time = [[x, "" if math.isnan(y) else y] for x, y in zip_time_pairs]
-        return zip_vs_time
-
     def report_statistic(self):
+        """生成舒适性评估报告"""
         comfort_result = self.comf_statistic()
-
-        # comfort_config_path = self.config_path / "comfort_config.yaml" #"comfort_config.yaml"  # "comfort_config.yaml"
         evaluator = Score(self.data_processed.comfort_config)
         result = evaluator.evaluate(comfort_result) 
         print("\n[舒适性表现及得分情况]")
-        
         return result
-    
-if __name__ == '__main__':
-    case_name = 'ICA'
-    mode_label = 'PGVIL'
-    
-    data = data_process.DataPreprocessing(case_name, mode_label)
-
-
-    comfort_instance = Comfort(data)  
-    # 调用实例方法 report_statistic，它不接受除 self 之外的参数  
-    try:  
-        comfort_result = comfort_instance.report_statistic() 
-        result = {'comfort': comfort_result}
-        
-    except Exception as e:  
-        print(f"An error occurred in Comfort.report_statistic: {e}")

modules/metric/efficient.py

@@ -2,104 +2,149 @@
 # -*- coding: utf-8 -*-
 ##################################################################
 #
-# Copyright (c) 2024 CICV, Inc. All Rights Reserved
+# Copyright (c) 2023 CICV, Inc. All Rights Reserved
 #
 ##################################################################
 """
 @Authors:           xieguijin(xieguijin@china-icv.cn)
-@Data:              2024/12/23
-@Last Modified:     2024/12/23
+@Data:              2023/07/23
+@Last Modified:     2023/07/23
 @Summary:           Efficient metrics calculation
 """
 
 from modules.lib.score import Score
-from modules.lib import log_manager
+from modules.lib.log_manager import LogManager
 
 
 class Efficient:
-    STOP_SPEED_THRESHOLD = 0.05  # Speed threshold to consider as stop
-    STOP_TIME_THRESHOLD = 0.5  # Time threshold to consider as stop (in seconds)
-    FRAME_RANGE = 13  # Frame range to classify stop duration
+    """高效性指标计算类"""
     
     def __init__(self, data_processed):
+        """初始化高效性指标计算类
         
-        # self.logger = log.get_logger()  # 使用时再初始化
-       
+        Args:
+            data_processed: 预处理后的数据对象
+        """
+        self.logger = LogManager().get_logger()
         self.data_processed = data_processed
         self.df = data_processed.object_df.copy()
-        self.ego_df = data_processed.ego_data
-        self.stop_count = 0  # Initialize stop count
-        self.stop_duration = 0  # Initialize stop duration
+        self.ego_df = data_processed.ego_data.copy()
+        
+        # 配置参数
+        self.STOP_SPEED_THRESHOLD = 0.05  # 停车速度阈值 (m/s)
+        self.STOP_TIME_THRESHOLD = 0.5    # 停车时间阈值 (秒)
+        self.FRAME_RANGE = 13             # 停车帧数阈值
+        
+        # 初始化结果变量
+        self.stop_count = 0     # 停车次数
+        self.stop_duration = 0  # 平均停车时长
+        self.average_v = 0      # 平均速度
         
     def _max_speed(self):
-        """Return the maximum speed in the ego data."""
+        """计算最大速度
+        
+        Returns:
+            float: 最大速度 (m/s)
+        """
         return self.ego_df['v'].max()
 
     def _deviation_speed(self):
-        """Return the variance of the speed in the ego data."""
+        """计算速度方差
+        
+        Returns:
+            float: 速度方差
+        """
         return self.ego_df['v'].var()
 
     def average_velocity(self):
-        """
-        Calculate the average velocity of the ego data.
-        Average velocity = total distance / total time
+        """计算平均速度
+        
+        Returns:
+            float: 平均速度 (m/s)
         """
         self.average_v = self.ego_df['v'].mean()
         return self.average_v
 
     def stop_duration_and_count(self):
+        """计算停车次数和平均停车时长
+        
+        Returns:
+            float: 平均停车时长 (秒)
         """
-        Calculate stop duration and stop count based on the following:
-        - Stops are detected when speed is <= STOP_SPEED_THRESHOLD
-        - Stops should last more than STOP_TIME_THRESHOLD (in seconds)
-        """
-        stop_time_list = self.ego_df[self.ego_df['v'] <= self.STOP_SPEED_THRESHOLD]['simTime'].values.tolist()
-        stop_frame_list = self.ego_df[self.ego_df['v'] <= self.STOP_SPEED_THRESHOLD]['simFrame'].values.tolist()
-
+        # 获取速度低于阈值的时间和帧号
+        stop_mask = self.ego_df['v'] <= self.STOP_SPEED_THRESHOLD
+        if not any(stop_mask):
+            return 0  # 如果没有停车，直接返回0
+            
+        stop_time_list = self.ego_df.loc[stop_mask, 'simTime'].values.tolist()
+        stop_frame_list = self.ego_df.loc[stop_mask, 'simFrame'].values.tolist()
+        
+        if not stop_frame_list:
+            return 0  # 防止空列表导致的索引错误
+            
         stop_frame_group = []
         stop_time_group = []
         sum_stop_time = 0
-        f1, t1 = stop_frame_list[0] if stop_frame_list else 0, stop_time_list[0] if stop_time_list else 0
-
+        f1, t1 = stop_frame_list[0], stop_time_list[0]
+        
+        # 检测停车段
         for i in range(1, len(stop_frame_list)):
-            if stop_frame_list[i] - stop_frame_list[i - 1] != 1:  # Frame discontinuity
+            if stop_frame_list[i] - stop_frame_list[i - 1] != 1:  # 帧不连续
                 f2, t2 = stop_frame_list[i - 1], stop_time_list[i - 1]
-                # If stop is valid (frame gap >= FRAME_RANGE and duration > STOP_TIME_THRESHOLD)
+                # 如果停车有效（帧数差 >= FRAME_RANGE）
                 if f2 - f1 >= self.FRAME_RANGE:
                     stop_frame_group.append((f1, f2))
                     stop_time_group.append((t1, t2))
                     sum_stop_time += (t2 - t1)
                     self.stop_count += 1
-                # Update f1, t1
+                # 更新起始点
                 f1, t1 = stop_frame_list[i], stop_time_list[i]
-
-        # Check last stop segment
+        
+        # 检查最后一段停车
         if len(stop_frame_list) > 0:
             f2, t2 = stop_frame_list[-1], stop_time_list[-1]
-            if f2 - f1 >= self.FRAME_RANGE and f2 != self.ego_df['simFrame'].values[-1]:
+            last_frame = self.ego_df['simFrame'].values[-1]
+            # 确保不是因为数据结束导致的停车
+            if f2 - f1 >= self.FRAME_RANGE and f2 != last_frame:
                 stop_frame_group.append((f1, f2))
                 stop_time_group.append((t1, t2))
                 sum_stop_time += (t2 - t1)
                 self.stop_count += 1
-
-        # Calculate stop duration if stop count is not zero
-        self.stop_duration = sum_stop_time / self.stop_count if self.stop_count != 0 else 0
+        
+        # 计算平均停车时长
+        self.stop_duration = sum_stop_time / self.stop_count if self.stop_count > 0 else 0
+        
+        self.logger.info(f"检测到停车次数: {self.stop_count}, 平均停车时长: {self.stop_duration:.2f}秒")
         return self.stop_duration
 
     def report_statistic(self):
-        """Generate the statistics and report the results."""
+        """生成统计报告
+        
+        Returns:
+            dict: 高效性评估结果
+        """
+        # 计算各项指标
+        max_speed_ms = self._max_speed()
+        deviation_speed_ms = self._deviation_speed()
+        average_speed_ms = self.average_velocity()
+        
+        # 将 m/s 转换为 km/h 用于评分
+        max_speed_kmh = max_speed_ms * 3.6
+        deviation_speed_kmh = deviation_speed_ms * 3.6
+        average_speed_kmh = average_speed_ms * 3.6
+        
         efficient_result = {
-            'maxSpeed': self._max_speed(),
-            'deviationSpeed': self._deviation_speed(),
-            'averagedSpeed': self.average_velocity(),
+            'maxSpeed': max_speed_kmh,        # 转换为 km/h
+            'deviationSpeed': deviation_speed_kmh,  # 转换为 km/h
+            'averagedSpeed': average_speed_kmh,     # 转换为 km/h
             'stopDuration': self.stop_duration_and_count()
         }
-        # self.logger.info(f"Efficient metrics calculation completed. Results: {efficient_result}")
-        
         
+        self.logger.info(f"高效性指标计算完成，结果: {efficient_result}")
         
+        # 评分
         evaluator = Score(self.data_processed.efficient_config)
         result = evaluator.evaluate(efficient_result) 
-
+        
         print("\n[高效性表现及评价结果]")
         return result

modules/metric/safety.py

@@ -33,7 +33,7 @@ class Safe(object):
         self.data_processed = data_processed
 
         self.df = data_processed.object_df.copy()
-        self.ego_df = data_processed.ego_data
+        self.ego_df = data_processed.ego_data.copy()
         self.obj_id_list = data_processed.obj_id_list
         self.metric_list = [
             "TTC",
@@ -47,11 +47,11 @@ class Safe(object):
 
         # score data
         self.calculated_value = {
-            "TTC": 1.0,
-            "MTTC": 1.0,
-            "THW": 1.0,
+            "TTC": 10.0,
+            "MTTC": 10.0,
+            "THW": 10.0,
             # 'LonSD': 0.0,
-            "LatSD": 0.0,
+            "LatSD": 3.0,
             # 'DRAC': 0.0,
             "BTN": 1.0,
             # 'STN': 1.0,
@@ -124,15 +124,17 @@ class Safe(object):
         # self.empty_flag = True
         for frame_num in self.frame_list:
             ego_data = obj_dict[frame_num][EGO_PLAYER_ID]
-            v1 = ego_data["v"]  # km/h to m/s
+            v1 = ego_data["v"]  
             x1 = ego_data["posX"]
             y1 = ego_data["posY"]
             h1 = ego_data["posH"]
+            # 将角度转换为弧度
+           
             # len1 = ego_data['dimX']
             # width1 = ego_data['dimY']
             # o_x1 = ego_data['offX']
-            v_x1 = ego_data["speedX"]  # km/h to m/s
-            v_y1 = ego_data["speedY"]  # km/h to m/s
+            v_x1 = ego_data["speedX"]  
+            v_y1 = ego_data["speedY"] 
             a_x1 = ego_data["accelX"]
             a_y1 = ego_data["accelY"]
             # a1 = ego_data['accel']
@@ -152,10 +154,10 @@ class Safe(object):
                 dist = self.dist(x1, y1, x2, y2)
                 obj_data["dist"] = dist
 
-                v_x2 = obj_data["speedX"]  # km/h to m/s
-                v_y2 = obj_data["speedY"]  # km/h to m/s
+                v_x2 = obj_data["speedX"]  #  m/s
+                v_y2 = obj_data["speedY"]  #  m/s
 
-                v2 = obj_data["v"]  # km/h to m/s
+                v2 = obj_data["v"]  # m/s
                 # h2 = obj_data['posH']
                 # len2 = obj_data['dimX']
                 # width2 = obj_data['dimY']
@@ -166,32 +168,34 @@ class Safe(object):
 
                 dx, dy = x2 - x1, y2 - y1
 
-                # 定义矢量A和x轴正向向量x
-                A = np.array([dx, dy])
-                x = np.array([1, 0])
-
-                # 计算点积和向量长度
-                dot_product = np.dot(A, x)
-                vector_length_A = np.linalg.norm(A)
-                vector_length_x = np.linalg.norm(x)
-
-                # 计算夹角的余弦值
-                cos_theta = dot_product / (vector_length_A * vector_length_x)
-
-                # 将余弦值转换为角度值（弧度制）
-                beta = np.arccos(cos_theta)  # 如何通过theta正负确定方向
-
-                lon_d = dist * math.cos(beta - h1)
-                lat_d = abs(
-                    dist * math.sin(beta - h1)
-                )  # 需要增加左正右负的判断，但beta取值为[0,pi)
+                # 计算目标车相对于自车的方位角
+                beta = math.atan2(dy, dx)  # 添加beta的计算
+                
+                # 将全局坐标系下的相对位置向量转换到自车坐标系
+                # 自车坐标系：车头方向为x轴正方向，车辆左侧为y轴正方向
+                h1_rad = math.radians(90 - h1)  # 转换为与x轴的夹角
+                
+                # 坐标变换
+                lon_d = dx * math.cos(h1_rad) + dy * math.sin(h1_rad)  # 纵向距离（前为正，后为负）
+                lat_d = -dx * math.sin(h1_rad) + dy * math.cos(h1_rad)  # 横向距离（左为正，右为负）
+
+                # 添加调试信息
+                # if frame_num == self.frame_list[-1]:  # 判断是否是最后一帧
+                #     print(f"最后一帧数据 frame_num={frame_num}:")
+                #     print(f"目标车ID: {playerId}")
+                #     print(f"自车位置: ({x1:.2f}, {y1:.2f}), 朝向角: {h1:.2f}度")
+                #     print(f"目标车位置: ({x2:.2f}, {y2:.2f})")
+                #     print(f"相对距离: dx={dx:.2f}, dy={dy:.2f}")
+                #     print(f"纵向距离: lon_d={lon_d:.2f}")
+                #     print(f"横向距离: lat_d={lat_d:.2f}")
+                #     print("------------------------")
 
                 obj_dict[frame_num][playerId]["lon_d"] = lon_d
                 obj_dict[frame_num][playerId]["lat_d"] = lat_d
 
                 # 代码注释，这里是筛选出车前100米，车后5米，车右边4米的目标车，用于计算安全距离
                 # print(f"lon_d: {lon_d}, lat_d: {lat_d}")
-                if lon_d > 100 or lon_d < -5 or lat_d > 4:
+                if lon_d > 100 or lon_d < -5 or abs(lat_d) > 4:
                     continue
 
                 self.empty_flag = False
@@ -246,17 +250,34 @@ class Safe(object):
 
                 # DRAC = self._cal_DRAC(dist, vrel_projection_in_dist, len1, len2, width1, width2, o_x1, o_x2)
 
-                lon_a1 = a_x1 * math.cos(h1) + a_y1 * math.sin(h1)
-                lon_a2 = a_x2 * math.cos(h1) + a_y2 * math.sin(h1)
+                # 将加速度和速度投影到车辆坐标系
+                h1_rad_for_projection = math.radians(90 - h1)  # 与L175保持一致
+                
+                # 计算纵向加速度
+                lon_a1 = a_x1 * math.cos(h1_rad_for_projection) + a_y1 * math.sin(h1_rad_for_projection)
+                lon_a2 = a_x2 * math.cos(h1_rad_for_projection) + a_y2 * math.sin(h1_rad_for_projection)
                 lon_a = abs(lon_a1 - lon_a2)
-                lon_d = dist * abs(math.cos(beta - h1))
-                lon_v = v_x1 * math.cos(h1) + v_y1 * math.sin(h1)
+                
+                # 使用已计算的纵向距离或重新计算
+                # 选项1: 使用已计算的lon_d (推荐，保持一致性)
+                # lon_d = obj_dict[frame_num][playerId]["lon_d"]
+                
+                # 选项2: 重新计算，确保与L178一致
+                # lon_d = dx * math.cos(h1_rad_for_projection) + dy * math.sin(h1_rad_for_projection)
+                
+                # 选项3: 使用简化公式，但确保角度单位一致
+                lon_d = max(dist * abs(math.cos(beta - h1_rad_for_projection)), 0.1)  # 设置最小值避免除零
+                
+                # 计算纵向速度
+                lon_v = v_x1 * math.cos(h1_rad_for_projection) + v_y1 * math.sin(h1_rad_for_projection)
+                
+                # 计算BTN
                 BTN = self._cal_BTN_new(lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max)
 
                 lat_a1 = a_x1 * math.sin(h1) * -1 + a_y1 * math.cos(h1)
                 lat_a2 = a_x2 * math.sin(h1) * -1 + a_y2 * math.cos(h1)
                 lat_a = abs(lat_a1 - lat_a2)
-                lat_d = dist * abs(math.sin(beta - h1))
+                lat_d = dist * abs(math.sin(beta - h1_rad))
                 lat_v = v_x1 * math.sin(h1) * -1 + v_y1 * math.cos(h1)
                 # STN = self._cal_STN_new(TTC, lat_a1, lat_a, lat_d, lat_v, ego_decel_lat_max, width1, width2)
 
@@ -266,6 +287,9 @@ class Safe(object):
                 TTC = None if (not TTC or TTC < 0) else TTC
                 MTTC = None if (not MTTC or MTTC < 0) else MTTC
                 THW = None if (not THW or THW < 0) else THW
+                # print(f'TTC: {TTC if TTC is None else f"{TTC:.2f}"}, '
+                #       f'MTTC: {MTTC if MTTC is None else f"{MTTC:.2f}"}, '
+                #       f'THW: {THW if THW is None else f"{THW:.2f}"}')
 
                 # 原DRAC计算已被注释，需移除相关代码引用
                 # DRAC = 10 if DRAC >= 10 else DRAC  # 删除这行
@@ -353,6 +377,7 @@ class Safe(object):
 
         # 计算 A 相对于 B 在 AB 连线上的速度 V_on_AB
         V_on_AB = vx * U_ABx + vy * U_ABy
+        # print(f'_cal_v_projection:{V_on_AB}')
 
         return V_on_AB
 
@@ -436,17 +461,20 @@ class Safe(object):
 
     # TTC (time to collision)
     def _cal_TTC(self, dist, vrel_projection_in_dist):
-        if vrel_projection_in_dist == 0:
-            return math.inf
+        # 如果相对速度接近0，视为相对静止或无法计算TTC
+        if abs(vrel_projection_in_dist) < 0.02: # 使用之前讨论过的阈值判断
+            return self.calculated_value["TTC"] # 返回默认值10.0
         TTC = dist / vrel_projection_in_dist
+        # 如果计算结果为负，表示正在远离，也视为安全
+        if TTC < 0:
+            return self.calculated_value["TTC"] # 返回默认值10.0
         return TTC
 
     def _cal_MTTC(self, dist, vrel_projection_in_dist, arel_projection_in_dist):
-        MTTC = math.nan
         if arel_projection_in_dist != 0:
             tmp = vrel_projection_in_dist**2 + 2 * arel_projection_in_dist * dist
             if tmp < 0:
-                return math.nan
+                return self.calculated_value["MTTC"] # 返回默认值10.0
             t1 = (
                 -1 * vrel_projection_in_dist - math.sqrt(tmp)
             ) / arel_projection_in_dist
@@ -462,16 +490,21 @@ class Safe(object):
                 MTTC = t1
             elif t1 <= 0 and t2 > 0:
                 MTTC = t2
-        if arel_projection_in_dist == 0 and vrel_projection_in_dist > 0:
+            else:
+                return self.calculated_value["MTTC"] # 返回默认值10.0
+        elif arel_projection_in_dist == 0 and vrel_projection_in_dist > 0:
             MTTC = dist / vrel_projection_in_dist
+        else:
+            return self.calculated_value["MTTC"] # 返回默认值10.0
         return MTTC
 
     # THW (time headway)
     def _cal_THW(self, dist, v_ego_projection_in_dist):
-        if not v_ego_projection_in_dist:
-            THW = None
-        else:
-            THW = dist / v_ego_projection_in_dist
+        if not v_ego_projection_in_dist or abs(v_ego_projection_in_dist) < 0.02:
+            return self.calculated_value["THW"] # 返回默认值10.0
+        THW = dist / v_ego_projection_in_dist
+        if THW < 0:
+            return self.calculated_value["THW"] # 返回默认值10.0
         return THW
 
     def velocity(self, v_x, v_y):
@@ -499,6 +532,10 @@ class Safe(object):
         a_left_lat_brake_min,
         a_lat_max,
     ):
+        # 检查除数是否为零
+        if a_right_lat_brake_min == 0 or a_left_lat_brake_min == 0:
+            return self.calculated_value["LatSD"]  # 返回默认值3.0
+            
         v_right_rho = v_right + rho * a_lat_max
         v_left_rho = v_left + rho * a_lat_max
         dist_min = lat_dist + (
@@ -517,19 +554,29 @@ class Safe(object):
         if dist_length < 0:
             dist_width = dist - (width2 / 2 + width1 / 2)
             if dist_width < 0:
-                return math.inf
+                return self.calculated_value["DRAC"] if "DRAC" in self.calculated_value else 0.0  # 使用默认值
             else:
                 d = dist_width
         else:
             d = dist_length
+            
+        # 避免除零
+        if d <= 0:
+            return self.calculated_value["DRAC"] if "DRAC" in self.calculated_value else 0.0  # 使用默认值
+            
         DRAC = vrel_projection_in_dist**2 / (2 * d)
         return DRAC
 
     # BTN (brake threat number)
     def _cal_BTN_new(self, lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max):
+        # 安全检查，避免除零错误
+        if lon_d <= 0 or ego_decel_lon_max == 0:
+            return self.calculated_value["BTN"]  # 返回默认值1.0
+            
         BTN = (
             lon_a1 + lon_a - lon_v**2 / (2 * lon_d)
-        ) / ego_decel_lon_max  # max_ay为此车可实现的最大纵向加速度，目前为本次实例里的最大值
+        ) / ego_decel_lon_max
+        
         return BTN
 
     # STN (steer threat number)
@@ -618,16 +665,34 @@ class Safe(object):
         for metric in min_list:
             if metric in self.metric_list:
                 if metric in self.df.columns:
-                    self.calculated_value[metric] = self.df[metric].min()
+                    # 筛选出非NaN的值
+                    valid_values = self.df[metric].dropna()
+                    if not valid_values.empty:
+                        # 如果有非NaN值，计算最小值
+                        self.calculated_value[metric] = valid_values.min()
+                        print(f'self.calculated_value[{metric}]:{self.calculated_value[metric]}')
+                    else:
+                        # 如果全是NaN或列为空，赋默认值
+                        self.calculated_value[metric] = self.calculated_value[metric]
                 else:
-                    self.calculated_value[metric] = 10.0
+                    # 列不存在，赋默认值
+                    self.calculated_value[metric] = self.calculated_value[metric]
 
         for metric in max_list:
             if metric in self.metric_list:
                 if metric in self.df.columns:
-                    self.calculated_value[metric] = self.df[metric].max()
+                     # 筛选出非NaN的值
+                    valid_values = self.df[metric].dropna()
+                    if not valid_values.empty:
+                        # 如果有非NaN值，计算最大值
+                        self.calculated_value[metric] = valid_values.max()
+                        print(f'self.calculated_value[{metric}]:{self.calculated_value[metric]}')
+                    else:
+                         # 如果全是NaN或列为空，赋默认值
+                        self.calculated_value[metric] = self.calculated_value[metric]
                 else:
-                    self.calculated_value[metric] = 10.0
+                     # 列不存在，赋默认值
+                    self.calculated_value[metric] = self.calculated_value[metric]
         return self.calculated_value
 
     def _safe_no_obj_statistic(self):

modules/metric/traffic.py

@@ -731,7 +731,7 @@ class WrongWayViolation:
     def __init__(self, df_data):
         print("停车违规类初始化中...")
         self.traffic_violations_type = "停车违规类"
-        self.data = df_data.obj_data[1]
+        self.data = df_data.obj_data[1].copy()
         # 初始化违规统计
         self.violation_count = {
             "urbanExpresswayOrHighwayDrivingLaneStopped": 0,
@@ -791,7 +791,6 @@ class WrongWayViolation:
         # self.logger.info(f"停车违规类指标统计完成，统计结果：{self.violation_count}")
         return self.violation_count
 
-
 class SpeedingViolation(object):
     """超速违规类"""
 
@@ -800,9 +799,7 @@ class SpeedingViolation(object):
     def __init__(self, df_data):
         print("超速违规类初始化中...")
         self.traffic_violations_type = "超速违规类"
-        self.data = df_data.obj_data[
-            1
-        ]  # Copy to avoid modifying the original DataFrame
+        self.data = df_data.obj_data[1].copy()
         # 初始化违规统计
         self.violation_counts = {
             "urbanExpresswayOrHighwaySpeedOverLimit50": 0,
@@ -818,9 +815,11 @@ class SpeedingViolation(object):
         # 提取有效道路类型
         urban_expressway_or_highway = {1, 2}  # 使用大括号直接创建集合
         general_road = {3}  # 直接创建包含一个元素的集合
+        
+        
+        # 转换速度
         self.data["v"] *= 3.6  # 转换速度
-
-        # 违规判定
+        
         conditions = [
             (
                 self.data["road_fc"].isin(urban_expressway_or_highway)
@@ -867,14 +866,17 @@ class SpeedingViolation(object):
 
         # 统计各类违规情况
         self.violation_counts = self.data["violation_type"].value_counts().to_dict()
+        
+       
 
-    def statistic(self) -> str:
+    # 添加statistic方法
+    def statistic(self):
+        """返回统计结果"""
         # 处理数据
         self.process_violations()
-        # self.logger.info(f"超速违规类指标统计完成，统计结果：{self.violation_counts}")
+        print(f"超速违规类指标统计完成，统计结果：{self.violation_counts}")
         return self.violation_counts
 
-
 class TrafficLightViolation(object):
     """违反交通灯类"""
 

scripts/evaluator_test.py

@@ -189,31 +189,45 @@ def main():
     parser.add_argument(
         "--logPath",
         type=Path,
-        default="/home/kevin/kevin/zhaoyuan/zhaoyuan/log/runtime.log",
+        default="log/runtime.log",
         help="日志文件存储路径",
     )
     parser.add_argument(
         "--dataPath",
         type=Path,
-        default="/home/kevin/kevin/zhaoyuan/sqlite3_demo/docker_build/zhaoyuan_0320/V2V_CSAE53-2020_ForwardCollision_LST_02-03",
+        default="data/V2V_CSAE53-2020_ForwardCollisionW_LST_01-01",
         help="预处理后的输入数据目录",
     )
     parser.add_argument(
         "--configPath",
         type=Path,
-        default="/home/kevin/kevin/zhaoyuan/sqlite3_demo/docker_build/zhaoyuan_0320/config/metric_config.yaml",
+        default="config/metric_config.yaml",
         help="评估指标配置文件路径",
     )
     parser.add_argument(
         "--reportPath",
         type=Path,
 
-        default="/home/kevin/kevin/zhaoyuan/sqlite3_demo/docker_build/zhaoyuan_0320/result",
+        default="result",
         help="评估报告输出目录",
     )
     args = parser.parse_args()
 
     try:
+        ##############################################
+        # 新增：动态生成日志路径
+        ##############################################
+        # 从dataPath提取用例名称
+        data_name = args.dataPath.name  # 获取数据目录名称
+        log_dir = args.logPath.parent    # 保持原日志目录配置
+        
+        # 创建日志目录（如果不存在）
+        log_dir.mkdir(parents=True, exist_ok=True)
+        
+        # 生成新的日志路径：日志目录/数据名称.log
+        new_log_path = log_dir / f"{data_name}.log"
+        args.logPath = new_log_path      # 覆盖原logPath参数
+        ##############################################
         pipeline = EvaluationPipeline(
             args.configPath, args.logPath, args.dataPath, args.reportPath
         )

testdisk.log

@@ -1,45 +0,0 @@
-
-
-Wed Apr  9 16:48:34 2025
-Command line: TestDisk
-
-TestDisk 7.1, Data Recovery Utility, July 2019
-Christophe GRENIER <grenier@cgsecurity.org>
-https://www.cgsecurity.org
-OS: Linux, kernel 6.8.0-57-generic (#59~22.04.1-Ubuntu SMP PREEMPT_DYNAMIC Wed Mar 19 17:07:41 UTC 2) x86_64
-Compiler: GCC 11.2
-ext2fs lib: 1.46.5, ntfs lib: libntfs-3g, reiserfs lib: none, ewf lib: none, curses lib: ncurses 6.3
-/dev/sda: LBA, HPA, LBA48, DCO support
-/dev/sda: size       3907029168 sectors
-/dev/sda: user_max   3907029168 sectors
-/dev/sda: native_max 3907029168 sectors
-/dev/sda: dco        3907029168 sectors
-Warning: can't get size for Disk /dev/mapper/control - 0 B - 0 sectors, sector size=512
-Warning: can't get size for Disk /dev/loop14 - 0 B - 0 sectors, sector size=512
-Hard disk list
-Disk /dev/sda - 2000 GB / 1863 GiB - CHS 243201 255 63, sector size=512 - TOSHIBA DT01ACA200, S/N:Y1DAD0XGS, FW:MX4OAD30
-Disk /dev/loop0 - 4096 B - 8 sectors (RO), sector size=512
-Disk /dev/loop1 - 77 MB / 73 MiB - 151328 sectors (RO), sector size=512
-Disk /dev/loop10 - 46 MB / 44 MiB - 91008 sectors (RO), sector size=512
-Disk /dev/loop11 - 46 MB / 44 MiB - 91024 sectors (RO), sector size=512
-Disk /dev/loop12 - 581 KB / 568 KiB - 1136 sectors (RO), sector size=512
-Disk /dev/loop13 - 462 KB / 452 KiB - 904 sectors (RO), sector size=512
-Disk /dev/loop2 - 77 MB / 73 MiB - 151312 sectors (RO), sector size=512
-Disk /dev/loop3 - 270 MB / 258 MiB - 528984 sectors (RO), sector size=512
-Disk /dev/loop4 - 254 MB / 242 MiB - 496448 sectors (RO), sector size=512
-Disk /dev/loop5 - 529 MB / 505 MiB - 1034424 sectors (RO), sector size=512
-Disk /dev/loop6 - 541 MB / 516 MiB - 1056784 sectors (RO), sector size=512
-Disk /dev/loop7 - 96 MB / 91 MiB - 187776 sectors (RO), sector size=512
-Disk /dev/loop8 - 12 MB / 12 MiB - 24984 sectors (RO), sector size=512
-Disk /dev/loop9 - 12 MB / 12 MiB - 25240 sectors (RO), sector size=512
-Disk /dev/nvme0n1 - 1000 GB / 931 GiB - CHS 953869 64 32, sector size=512
-
-Partition table type (auto): EFI GPT
-Disk /dev/sda - 2000 GB / 1863 GiB - TOSHIBA DT01ACA200
-Partition table type: Intel
-
-Interface Advanced
-Geometry from i386 MBR: head=256 sector=63
-check_part_i386 1 type EE: no test
- 1 P EFI GPT                  0   0  2 243201  80 63 3907029167
-SIGINT detected! TestDisk has been killed.