zhaoyuan/modules/metric/safety.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
安全指标计算模块
"""

import numpy as np
import pandas as pd
import math
from collections import defaultdict
from typing import Dict, Any, List, Optional
import ast

from modules.lib.score import Score
from modules.lib.log_manager import LogManager

# 安全指标相关常量
SAFETY_INFO = [
    "simTime",
    "simFrame",
    "playerId",
    "posX",
    "posY",
    "posH",
    "speedX",
    "speedY",
    "accelX",
    "accelY",
    "v",
    "type",
    "lane_width",
    "lane_type",
    "road_type",
    "lane_coords",
    "link_coords"
]

# ----------------------
# 独立指标计算函数
# ----------------------
def calculate_ttc(data_processed) -> dict:
    """计算TTC (Time To Collision)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"TTC": None}
    try:
        safety = SafetyCalculator(data_processed)
        ttc_value = safety.get_ttc_value()
        LogManager().get_logger().info(f"安全指标[TTC]计算结果: {ttc_value}")
        return {"TTC": ttc_value}
    except Exception as e:
        LogManager().get_logger().error(f"TTC计算异常: {str(e)}", exc_info=True)
        return {"TTC": None}

def calculate_mttc(data_processed) -> dict:
    """计算MTTC (Modified Time To Collision)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"MTTC": None}
    try:
        safety = SafetyCalculator(data_processed)
        mttc_value = safety.get_mttc_value()
        LogManager().get_logger().info(f"安全指标[MTTC]计算结果: {mttc_value}")
        return {"MTTC": mttc_value}
    except Exception as e:
        LogManager().get_logger().error(f"MTTC计算异常: {str(e)}", exc_info=True)
        return {"MTTC": None}

def calculate_thw(data_processed) -> dict:
    """计算THW (Time Headway)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"THW": None}
    try:
        safety = SafetyCalculator(data_processed)
        thw_value = safety.get_thw_value()
        LogManager().get_logger().info(f"安全指标[THW]计算结果: {thw_value}")
        return {"THW": thw_value}
    except Exception as e:
        LogManager().get_logger().error(f"THW计算异常: {str(e)}", exc_info=True)
        return {"THW": None}

def calculate_tlc(data_processed) -> dict:
    """计算TLC (Time to Line Crossing)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"TLC": None}
    try:
        safety = SafetyCalculator(data_processed)
        tlc_value = safety.get_tlc_value()
        LogManager().get_logger().info(f"安全指标[TLC]计算结果: {tlc_value}")
        return {"TLC": tlc_value}
    except Exception as e:
        LogManager().get_logger().error(f"TLC计算异常: {str(e)}", exc_info=True)
        return {"TLC": None}

def calculate_ttb(data_processed) -> dict:
    """计算TTB (Time to Brake)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"TTB": None}
    try:
        safety = SafetyCalculator(data_processed)
        ttb_value = safety.get_ttb_value()
        LogManager().get_logger().info(f"安全指标[TTB]计算结果: {ttb_value}")
        return {"TTB": ttb_value}
    except Exception as e:
        LogManager().get_logger().error(f"TTB计算异常: {str(e)}", exc_info=True)
        return {"TTB": None}

def calculate_tm(data_processed) -> dict:
    """计算TM (Time Margin)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"TM": None}
    try:
        safety = SafetyCalculator(data_processed)
        tm_value = safety.get_tm_value()
        LogManager().get_logger().info(f"安全指标[TM]计算结果: {tm_value}")
        return {"TM": tm_value}
    except Exception as e:
        LogManager().get_logger().error(f"TM计算异常: {str(e)}", exc_info=True)
        return {"TM": None}

# def calculate_MPrTTC(data_processed) -> dict:
#     """计算MPrTTC (Model Predictive Time-to-Collision)"""
#     if data_processed is None or not hasattr(data_processed, 'object_df'):
#         return {"MPrTTC": None}
#     try:
#         safety = SafetyCalculator(data_processed)
#         mprttc_value = safety.get_mprttc_value()
#         LogManager().get_logger().info(f"安全指标[MPrTTC]计算结果: {mprttc_value}")
#         return {"MPrTTC": mprttc_value}
#     except Exception as e:
#         LogManager().get_logger().error(f"MPrTTC计算异常: {str(e)}", exc_info=True)
#         return {"MPrTTC": None}

def calculate_dtc(data_processed) -> dict:
    """计算DTC (Distance to Collision)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"DTC": None}
    try:
        safety = SafetyCalculator(data_processed)
        dtc_value = safety.get_dtc_value()
        LogManager().get_logger().info(f"安全指标[DTC]计算结果: {dtc_value}")
        return {"DTC": dtc_value}
    except Exception as e:
        LogManager().get_logger().error(f"DTC计算异常: {str(e)}", exc_info=True)
        return {"DTC": None}

def calculate_pet(data_processed) -> dict:
    """计算PET (Post Encroachment Time)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"PET": None}
    try:
        safety = SafetyCalculator(data_processed)
        pet_value = safety.get_dtc_value()
        LogManager().get_logger().info(f"安全指标[PET]计算结果: {pet_value}")
        return {"PET": pet_value}
    except Exception as e:
        LogManager().get_logger().error(f"PET计算异常: {str(e)}", exc_info=True)
        return {"PET": None}

def calculate_psd(data_processed) -> dict:
    """计算PSD (Potential Safety Distance)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"PSD": None}
    try:
        safety = SafetyCalculator(data_processed)
        psd_value = safety.get_psd_value()
        LogManager().get_logger().info(f"安全指标[PSD]计算结果: {psd_value}")
        return {"PSD": psd_value}
    except Exception as e:
        LogManager().get_logger().error(f"PSD计算异常: {str(e)}", exc_info=True)
        return {"PSD": None}

def calculate_collisionrisk(data_processed) -> dict:
    """计算碰撞风险"""
    safety = SafetyCalculator(data_processed)
    collision_risk_value = safety.get_collision_risk_value()
    LogManager().get_logger().info(f"安全指标[collisionRisk]计算结果: {collision_risk_value}")
    return {"collisionRisk": collision_risk_value}

def calculate_lonsd(data_processed) -> dict:
    """计算纵向安全距离"""
    safety = SafetyCalculator(data_processed)
    lonsd_value = safety.get_lonsd_value()
    LogManager().get_logger().info(f"安全指标[LonSD]计算结果: {lonsd_value}")
    return {"LonSD": lonsd_value}

def calculate_latsd(data_processed) -> dict:
    """计算横向安全距离"""
    safety = SafetyCalculator(data_processed)
    latsd_value = safety.get_latsd_value()
    LogManager().get_logger().info(f"安全指标[LatSD]计算结果: {latsd_value}")
    return {"LatSD": latsd_value}

def calculate_btn(data_processed) -> dict:
    """计算制动威胁数"""
    safety = SafetyCalculator(data_processed)
    btn_value = safety.get_btn_value()
    LogManager().get_logger().info(f"安全指标[BTN]计算结果: {btn_value}")
    return {"BTN": btn_value}

def calculate_collisionseverity(data_processed) -> dict:
    """计算碰撞严重性"""
    safety = SafetyCalculator(data_processed)
    collision_severity_value = safety.get_collision_severity_value()
    LogManager().get_logger().info(f"安全指标[collisionSeverity]计算结果: {collision_severity_value}")
    return {"collisionSeverity": collision_severity_value}


class SafetyRegistry:
    """安全指标注册器"""
    
    def __init__(self, data_processed):
        self.logger = LogManager().get_logger()
        self.data = data_processed
        self.safety_config = data_processed.safety_config["safety"]
        self.metrics = self._extract_metrics(self.safety_config)
        self._registry = self._build_registry()
    
    def _extract_metrics(self, config_node: dict) -> list:
        """从配置中提取指标名称"""
        metrics = []
        def _recurse(node):
            if isinstance(node, dict):
                if 'name' in node and not any(isinstance(v, dict) for v in node.values()):
                    metrics.append(node['name'])
                for v in node.values():
                    _recurse(v)
        _recurse(config_node)
        self.logger.info(f'评比的安全指标列表:{metrics}')
        return metrics
    
    def _build_registry(self) -> dict:
        """构建指标函数注册表"""
        registry = {}
        for metric_name in self.metrics:
            func_name = f"calculate_{metric_name.lower()}"
            if func_name in globals():
                registry[metric_name] = globals()[func_name]
            else:
                self.logger.warning(f"未实现安全指标函数: {func_name}")
        return registry
    
    def batch_execute(self) -> dict:
        """批量执行指标计算"""
        results = {}
        for name, func in self._registry.items():
            try:
                result = func(self.data)
                results.update(result)
            except Exception as e:
                self.logger.error(f"{name} 执行失败: {str(e)}", exc_info=True)
                results[name] = None
        self.logger.info(f'安全指标计算结果:{results}')
        return results


class SafeManager:
    """安全指标管理类"""
    
    def __init__(self, data_processed):
        self.data = data_processed
        self.registry = SafetyRegistry(self.data)
    
    def report_statistic(self):
        """计算并报告安全指标结果"""
        safety_result = self.registry.batch_execute()
        
        return safety_result
    

class SafetyCalculator:
    """安全指标计算类 - 兼容Safe类风格"""

    def __init__(self, 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.copy()  # 使用copy()避免修改原始数据
        self.obj_id_list = data_processed.obj_id_list

        self.metric_list = [
            'TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'TM', 'DTC', 'PET', 'PSD', 'LonSD', 'LatSD', 'BTN', 'collisionRisk', 'collisionSeverity'
        ]

        # 初始化默认值
        self.calculated_value = {
            "TTC": 10.0,
            "MTTC": 10.0,
            "THW": 10.0,
            "TLC": 10.0,
            "TTB": 10.0,
            "TM": 10.0,
            # "MPrTTC": 10.0,
            "PET": 10.0,
            "DTC": 10.0,
            "PSD": 10.0,
            "LatSD": 3.0,
            "BTN": 1.0,
            "collisionRisk": 0.0,
            "collisionSeverity": 0.0,
        }

        self.time_list = self.ego_df['simTime'].values.tolist()
        self.frame_list = self.ego_df['simFrame'].values.tolist()
        self.collisionRisk = 0
        self.empty_flag = True

        self.logger.info("SafetyCalculator初始化完成，场景中包含自车的目标物一共为: %d", len(self.obj_id_list))

        if len(self.obj_id_list) > 1:
            self.unsafe_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_time_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_dist_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_acce_drac_df = pd.DataFrame(
                columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_acce_xtn_df = pd.DataFrame(
                columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_prob_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.most_dangerous = {}
            self.pass_percent = {}
            self.logger.info("开始执行安全参数计算 _safe_param_cal_new")
            self._safe_param_cal_new()
            self.logger.info("安全参数计算完成")

    def _safe_param_cal_new(self):
        self.logger.debug("进入 _safe_param_cal_new 方法")
        # 直接复用Safe类的实现
        Tc = 0.3  # 安全距离

        rho = self.data_processed.vehicle_config["RHO"]
        ego_accel_max = self.data_processed.vehicle_config["EGO_ACCEL_MAX"]
        obj_decel_max = self.data_processed.vehicle_config["OBJ_DECEL_MAX"]
        ego_decel_min = self.data_processed.vehicle_config["EGO_DECEL_MIN"]
        ego_decel_lon_max = self.data_processed.vehicle_config["EGO_DECEL_LON_MAX"]
        ego_decel_lat_max = self.data_processed.vehicle_config["EGO_DECEL_LAT_MAX"]
        driver_reaction_time = self.data_processed.vehicle_config["RHO"]
        ego_decel_max = np.sqrt(ego_decel_lon_max ** 2 + ego_decel_lat_max ** 2)
        x_relative_start_dist = self.ego_df["x_relative_dist"]

        obj_dict = defaultdict(dict)
        obj_data_dict = self.df.to_dict('records')
        for item in obj_data_dict:
            obj_dict[item['simFrame']][item['playerId']] = item

        df_list = []
        EGO_PLAYER_ID = 1
        ramp_poss = self.ego_df[self.ego_df["link_type"] == 19]["link_coords"].drop_duplicates().tolist() # 寻找匝道的位置坐标
        lane_poss = self.ego_df[self.ego_df["lane_type"] == 2]["lane_coords"].drop_duplicates().tolist() # 寻找匝道的位置坐标
        for frame_num in self.frame_list:
            ego_data = obj_dict[frame_num][EGO_PLAYER_ID]
            v1 = ego_data['v']
            x1 = ego_data['posX']
            y1 = ego_data['posY']
            h1 = ego_data['posH']
            laneOffset = ego_data["laneOffset"]

            v_x1 = ego_data['speedX']
            v_y1 = ego_data['speedY']
            a_x1 = ego_data['accelX']
            a_y1 = ego_data['accelY']
            a1 = np.sqrt(a_x1 ** 2 + a_y1 ** 2)

            for playerId in self.obj_id_list:
                if playerId == EGO_PLAYER_ID:
                    continue
                try:
                    obj_data = obj_dict[frame_num][playerId]
                except KeyError:
                    continue

                x2 = obj_data['posX']
                y2 = obj_data['posY']
                dist = self.dist(x1, y1, x2, y2)
                obj_data['dist'] = dist

                v_x2 = obj_data['speedX']
                v_y2 = obj_data['speedY']
                v2 = obj_data['v']
                a_x2 = obj_data['accelX']
                a_y2 = obj_data['accelY']
                a2 = np.sqrt(a_x2 ** 2 + a_y2 ** 2)

                dx, dy = x2 - x1, y2 - y1
                
                # 计算目标车相对于自车的方位角
                beta = math.atan2(dy, dx)
                
                # 将全局坐标系下的相对位置向量转换到自车坐标系
                # 自车坐标系：车头方向为x轴正方向，车辆左侧为y轴正方向
                h1_rad = math.radians(90 - h1)  # 转换为与x轴的夹角
                
                # 坐标变换
                lon_d = dx * math.cos(h1_rad) + dy * math.sin(h1_rad)  # 纵向距离（前为正，后为负）
                lat_d = abs(-dx * math.sin(h1_rad) + dy * math.cos(h1_rad))  # 横向距离（取绝对值）
                
                obj_dict[frame_num][playerId]['lon_d'] = lon_d
                obj_dict[frame_num][playerId]['lat_d'] = lat_d

                if lon_d > 100 or lon_d < -5 or lat_d > 4:
                    continue

                self.empty_flag = False

                vx, vy = v_x1 - v_x2, v_y1 - v_y2
                ax, ay = a_x2 - a_x1, a_y2 - a_y1
                relative_v = np.sqrt(vx ** 2 + vy ** 2)

                v_ego_p = self._cal_v_ego_projection(dx, dy, v_x1, v_y1)
                v_obj_p = self._cal_v_ego_projection(dx, dy, v_x2, v_y2)
                vrel_projection_in_dist = self._cal_v_projection(dx, dy, vx, vy)
                arel_projection_in_dist = self._cal_a_projection(dx, dy, vx, vy, ax, ay, x1, y1, x2, y2, v_x1, v_y1,
                                                                 v_x2, v_y2)

                obj_dict[frame_num][playerId]['vrel_projection_in_dist'] = vrel_projection_in_dist
                obj_dict[frame_num][playerId]['arel_projection_in_dist'] = arel_projection_in_dist
                obj_dict[frame_num][playerId]['v_ego_projection_in_dist'] = v_ego_p
                obj_dict[frame_num][playerId]['v_obj_projection_in_dist'] = v_obj_p

                obj_type = obj_data['type']

                TTC = self._cal_TTC(dist, vrel_projection_in_dist) if abs(vrel_projection_in_dist) > 1e-6 else None
                MTTC = self._cal_MTTC(dist, vrel_projection_in_dist, arel_projection_in_dist)
                THW = self._cal_THW(dist, v_ego_p) if abs(v_ego_p) > 1e-6 else None
                TLC = self._cal_TLC(v1, h1, laneOffset)
                TTB = self._cal_TTB(x_relative_start_dist, relative_v, ego_decel_max)
                TM = self._cal_TM(x_relative_start_dist, v2, a2, v1, a1)
                DTC = self._cal_DTC(vrel_projection_in_dist, arel_projection_in_dist, driver_reaction_time)
                # MPrTTC = self._cal_MPrTTC(x_relative_start_dist)
                # PET = self._cal_PET(lane_posx1, lane_posy1, lane_posx2, lane_posy2, ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, ego_posx, ego_posy, obj_posx, obj_posy, lane_width, delta_t, v1, v2, a1, a2)
                PET = None
                for lane_pos in lane_poss:
                    lane_posx1 = ast.literal_eval(lane_pos)[0][0]
                    lane_posy1 = ast.literal_eval(lane_pos)[0][1]
                    lane_posx2 = ast.literal_eval(lane_pos)[-1][0]
                    lane_posy2 = ast.literal_eval(lane_pos)[-1][1]
                    for ramp_pos in ramp_poss:
                        ramp_posx1 = ast.literal_eval(ramp_pos)[0][0]
                        ramp_posy1 = ast.literal_eval(ramp_pos)[0][1]
                        ramp_posx2 = ast.literal_eval(ramp_pos)[-1][0]
                        ramp_posy2 = ast.literal_eval(ramp_pos)[-1][1]
                        ego_posx = x1
                        ego_posy = y1
                        obj_posx = x2
                        obj_posy = y2
                        delta_t = self._cal_reaction_time_to_avgspeed(self.ego_df)
                        lane_width = self.ego_df["lane_width"].iloc[0]
                        PET = self._cal_PET(lane_posx1, lane_posy1, lane_posx2, lane_posy2, ramp_posx1, ramp_posy1, ramp_posx2,
                                            ramp_posy2, ego_posx, ego_posy, obj_posx, obj_posy, lane_width, delta_t, v1, v2, a1, a2)
                PSD = self._cal_PSD(x_relative_start_dist, v1, ego_decel_lon_max)



                LonSD = self._cal_longitudinal_safe_dist(v_ego_p, v_obj_p, rho, ego_accel_max, ego_decel_min, obj_decel_max)

                lat_dist = 0.5
                v_right = v1
                v_left = v2
                a_right_lat_brake_min = 1
                a_left_lat_brake_min = 1
                a_lat_max = 5

                LatSD = self._cal_lateral_safe_dist(lat_dist, v_right, v_left, rho, a_right_lat_brake_min,
                                                    a_left_lat_brake_min, a_lat_max)

                # 使用自车坐标系下的纵向加速度
                lon_a1 = a_x1 * math.cos(h1_rad) + a_y1 * math.sin(h1_rad)
                lon_a2 = a_x2 * math.cos(h1_rad) + a_y2 * math.sin(h1_rad)
                lon_a = abs(lon_a1 - lon_a2)
                lon_d = max(0.1, lon_d)  # 确保纵向距离为正
                lon_v = v_x1 * math.cos(h1_rad) + v_y1 * math.sin(h1_rad)
                BTN = self._cal_BTN_new(lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max)

                # 使用自车坐标系下的横向加速度
                lat_a1 = -a_x1 * math.sin(h1_rad) + a_y1 * math.cos(h1_rad)
                lat_a2 = -a_x2 * math.sin(h1_rad) + a_y2 * math.cos(h1_rad)
                lat_a = abs(lat_a1 - lat_a2)
                lat_v = -v_x1 * math.sin(h1_rad) + v_y1 * math.cos(h1_rad)

                obj_dict[frame_num][playerId]['lat_v_rel'] = lat_v - (-v_x2 * math.sin(h1_rad) + v_y2 * math.cos(h1_rad))
                obj_dict[frame_num][playerId]['lon_v_rel'] = lon_v - (v_x2 * math.cos(h1_rad) + v_y2 * math.sin(h1_rad))

                TTC = None if (TTC is None or TTC < 0) else TTC
                MTTC = None if (MTTC is None or MTTC < 0) else MTTC
                THW = None if (THW is None or THW < 0) else THW
                TLC = None if (TLC is None or TLC < 0) else TLC
                TTB = None if (TTB is None or TTB < 0) else TTB
                TM = None if (TM is None or TM < 0) else TM
                DTC = None if (DTC is None or DTC < 0) else DTC
                PET = None if (PET is None or PET < 0) else PET
                PSD = None if (PSD is None or PSD < 0) else PSD

                obj_dict[frame_num][playerId]['TTC'] = TTC
                obj_dict[frame_num][playerId]['MTTC'] = MTTC
                obj_dict[frame_num][playerId]['THW'] = THW
                obj_dict[frame_num][playerId]['TLC'] = TLC
                obj_dict[frame_num][playerId]['TTB'] = TTB
                obj_dict[frame_num][playerId]['TM'] = TM
                obj_dict[frame_num][playerId]['DTC'] = DTC
                obj_dict[frame_num][playerId]['PET'] = PET
                obj_dict[frame_num][playerId]['PSD'] = PSD
                obj_dict[frame_num][playerId]['LonSD'] = LonSD
                obj_dict[frame_num][playerId]['LatSD'] = LatSD
                obj_dict[frame_num][playerId]['BTN'] = abs(BTN)

                collisionSeverity = 0
                pr_death = 0
                collisionRisk = 0

                obj_dict[frame_num][playerId]['collisionSeverity'] = collisionSeverity * 100
                obj_dict[frame_num][playerId]['pr_death'] = pr_death * 100
                obj_dict[frame_num][playerId]['collisionRisk'] = collisionRisk * 100

            df_fnum = pd.DataFrame(obj_dict[frame_num].values())
            df_list.append(df_fnum)

        df_safe = pd.concat(df_list)
        col_list = ['simTime', 'simFrame', 'playerId',
                    'TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'TM', 'DTC', 'PET', 'PSD', 'LonSD', 'LatSD', 'BTN',
                    'collisionSeverity', 'pr_death', 'collisionRisk']
        self.df_safe = df_safe[col_list].reset_index(drop=True)

    # 计算车辆从非匀速达到匀速的反应时间
    def _cal_reaction_time_to_avgspeed(self, ego_df, threshold = 0.01):
        ego_df = ego_df.reset_index(drop=True)
        ego_df['v_change'] = ego_df['v'].diff()
        # 初始化结果列表
        uniform_speed_segments = []
        start_index = 0

        # 遍历数据，找出匀速部分
        for i in range(1, len(ego_df)):
            if ego_df['v_change'].iloc[i] > threshold:
                if i - start_index > 1:  # 至少有两个数据点才能形成一个匀速段
                    uniform_speed_segments.append(
                        (ego_df.iloc[start_index]['simTime'], ego_df.iloc[i - 1]['simTime'], ego_df.iloc[start_index]['v']))
                start_index = i

        # 检查最后一个段
        if len(ego_df) - start_index > 1:
            uniform_speed_segments.append(
                (ego_df.iloc[start_index]['simTime'], ego_df.iloc[-1]['simTime'], ego_df.iloc[start_index]['v']))
        return uniform_speed_segments[-1][0] - ego_df['simTime'].iloc[0]

    def _cal_v_ego_projection(self, dx, dy, v_x1, v_y1):
        # 计算 AB 连线的向量 AB
        # dx = x2 - x1
        # dy = y2 - y1

        # 计算 AB 连线的模长 |AB|
        AB_mod = math.sqrt(dx ** 2 + dy ** 2)

        # 计算 AB 连线的单位向量 U_AB
        U_ABx = dx / AB_mod
        U_ABy = dy / AB_mod

        # 计算 A 在 AB 连线上的速度 V1_on_AB
        V1_on_AB = v_x1 * U_ABx + v_y1 * U_ABy

        return V1_on_AB

    def _cal_v_projection(self, dx, dy, vx, vy):
        # 计算 AB 连线的向量 AB
        # dx = x2 - x1
        # dy = y2 - y1

        # 计算 AB 连线的模长 |AB|
        AB_mod = math.sqrt(dx ** 2 + dy ** 2)

        # 计算 AB 连线的单位向量 U_AB
        U_ABx = dx / AB_mod
        U_ABy = dy / AB_mod

        # 计算 A 相对于 B 的速度 V_relative
        # vx = vx1 - vx2
        # vy = vy1 - vy2

        # 计算 A 相对于 B 在 AB 连线上的速度 V_on_AB
        V_on_AB = vx * U_ABx + vy * U_ABy

        return V_on_AB

    def _cal_a_projection(self, dx, dy, vx, vy, ax, ay, x1, y1, x2, y2, v_x1, v_y1, v_x2, v_y2):
        # 计算 AB 连线的向量 AB
        # dx = x2 - x1
        # dy = y2 - y1

        # 计算 θ
        V_mod = math.sqrt(vx ** 2 + vy ** 2)
        AB_mod = math.sqrt(dx ** 2 + dy ** 2)
        if V_mod == 0 or AB_mod == 0:
            return 0

        cos_theta = (vx * dx + vy * dy) / (V_mod * AB_mod)
        theta = math.acos(cos_theta)

        # 计算 AB 连线的模长 |AB|
        AB_mod = math.sqrt(dx ** 2 + dy ** 2)

        # 计算 AB 连线的单位向量 U_AB
        U_ABx = dx / AB_mod
        U_ABy = dy / AB_mod

        # 计算 A 相对于 B 的加速度 a_relative
        # ax = ax1 - ax2
        # ay = ay1 - ay2

        # 计算 A 相对于 B 在 AB 连线上的加速度 a_on_AB
        a_on_AB = ax * U_ABx + ay * U_ABy

        VA = np.array([v_x1, v_y1])
        VB = np.array([v_x2, v_y2])
        D_A = np.array([x1, y1])
        D_B = np.array([x2, y2])
        V_r = VA - VB
        V = np.linalg.norm(V_r)
        w = self._cal_relative_angular_v(theta, D_A, D_B, VA, VB)
        a_on_AB_back = self._calculate_derivative(a_on_AB, w, V, theta)
        return a_on_AB_back

    # 计算相对加速度
    def _calculate_derivative(self, a, w, V, theta):
        # 计算(V×cos(θ))'的值
        # derivative = a * math.cos(theta) - w * V * math.sin(theta)theta
        derivative = a - w * V * math.sin(theta)
        return derivative

    def _cal_relative_angular_v(self, theta, A, B, VA, VB):
        dx = A[0] - B[0]
        dy = A[1] - B[1]
        dvx = VA[0] - VB[0]
        dvy = VA[1] - VB[1]
        # (dx * dvy - dy * dvx)
        angular_velocity = math.sqrt(dvx ** 2 + dvy ** 2) * math.sin(theta) / math.sqrt(dx ** 2 + dy ** 2)
        return angular_velocity

    def _death_pr(self, obj_type, v_relative):
        if obj_type == 5:
            p_death = 1 / (1 + np.exp(7.723 - 0.15 * v_relative))
        else:
            p_death = 1 / (1 + np.exp(8.192 - 0.12 * v_relative))
        return p_death

    def _cal_collisionRisk_level(self, obj_type, v_relative, collisionSeverity):
        if obj_type == 5:
            p_death = 1 / (1 + np.exp(7.723 - 0.15 * v_relative))
        else:
            p_death = 1 / (1 + np.exp(8.192 - 0.12 * v_relative))
        collisionRisk = 0.4 * p_death + 0.6 * collisionSeverity
        return collisionRisk

    # 求两车之间当前距离
    def dist(self, x1, y1, x2, y2):
        dist = np.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)
        return dist

    # 求车辆与道路之间的横向距离
    def horizontal_distance(self, posx1, posy1, posx2, posy2, posx, posy):
        dist = np.sqrt((posx2 - posx1)(posy - posy1) - (posy2 - posy1)(posx - posy1))/np.sqrt((posx2 - posx1)**2 + (posy2 - posy1)**2)
        return dist

    # 求车辆与道路之间的纵向距离
    def _is_alone_the_road(self, posx1, posy1, posx2, posy2, posx, posy):
        pro_car = (posx2 - posx1)(posx - posx1) + (posy2 - posy1)(posy - posy1)
        if pro_car > 0:
            return True
        else:
            return False

    def _is_in_the_road(self, posx1, posy1, posx2, posy2, posx, posy):
        pro_obj1 = (posx2 - posx1)(posx - posx1) + (posy2 - posy1)(posy - posy1)
        pro_obj2 = (posx1 - posx2)(posx - posx2) + (posy1 - posy2)(posy - posy2)
        if pro_obj1 > 0 and pro_obj2 > 0:
            return True
        else:
            return False

    # TTC (time to collision)
    def _cal_TTC(self, dist, vrel_projection_in_dist):
        if vrel_projection_in_dist == 0:
            return math.inf
        TTC = dist / vrel_projection_in_dist
        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
            t1 = (-1 * vrel_projection_in_dist - math.sqrt(tmp)) / arel_projection_in_dist
            t2 = (-1 * vrel_projection_in_dist + math.sqrt(tmp)) / arel_projection_in_dist
            if t1 > 0 and t2 > 0:
                if t1 >= t2:
                    MTTC = t2
                elif t1 < t2:
                    MTTC = t1
            elif t1 > 0 and t2 <= 0:
                MTTC = t1
            elif t1 <= 0 and t2 > 0:
                MTTC = t2
        if arel_projection_in_dist == 0 and vrel_projection_in_dist > 0:
            MTTC = dist / vrel_projection_in_dist
        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
        return THW
    # TLC (time to line crossing)
    def _cal_TLC(self, ego_v, ego_yaw, laneOffset):
        TLC = laneOffset/ego_v/np.sin(ego_yaw) if ((ego_v != 0) and (np.sin(ego_yaw) != 0)) else 10.0
        if TLC < 0:
            TLC = None
        return TLC

    def _cal_TTB(self, x_relative_start_dist, relative_v, ego_decel_max):
        if len(x_relative_start_dist):
            return None
        if (ego_decel_max  == 0) or (relative_v == 0):
            return self.calculated_value["TTB"]
        else:
            x_relative_start_dist0 = x_relative_start_dist.tolist()[0]
            TTB = (x_relative_start_dist0 + relative_v * relative_v/2/ego_decel_max)/relative_v
            return TTB

    def _cal_TM(self, x_relative_start_dist, v2, a2, v1, a1):
        if len(x_relative_start_dist):
            return None
        if (a2 == 0) or (v1 == 0):
            return self.calculated_value["TM"]
        if a1 == 0:
            return None
        x_relative_start_dist0 = x_relative_start_dist.tolist()[0]
        TM = (x_relative_start_dist0 + v2**2/(2*a2) - v1**2/(2*a1)) / v1
        return TM

    # def _cal_MPrTTC(self, T=5, c = False, collision_dist = 5.99):
    #     time_interval = self.ego_df['simTime'].tolist()[1] - self.ego_df['simTime'].tolist()[0]
    #
    #     for i in range(len(self.obj_id_list)):
    #         for j in range(T):
    #             MPrTTC = j * time_interval

    def _cal_DTC(self, v_on_dist, a_on_dist, t):
        if a_on_dist == 0:
            return None
        DTC = v_on_dist * t + v_on_dist ** 2 / a_on_dist
        return DTC

    def _cal_PET(self, lane_posx1, lane_posy1, lane_posx2, lane_posy2, ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, ego_posx, ego_posy, obj_posx, obj_posy, lane_width, delta_t, v1, v2, a1, a2):
        dist1 = self.horizontal_distance(lane_posx1, lane_posy1, lane_posx2, lane_posy2, ego_posx, ego_posy)
        dist2 = self.horizontal_distance(ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, obj_posx, obj_posy)
        if ((dist1 <= lane_width/2) and (self._is_alone_the_road(lane_posx1, lane_posy1, lane_posx2, lane_posy2, ego_posx, ego_posy))
                and (self._is_in_the_road(ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, obj_posx, obj_posy))
                and (dist2 <= lane_width/2) and (a1 != 0) and (a2 != 0)):
            dist_ego = np.sqrt((ego_posx - lane_posx1)**2 + (ego_posy-lane_posy1)**2)
            dist_obj = np.sqrt((obj_posx - ramp_posx2)**2 + (obj_posy-ramp_posy2)**2)
            PET = (-2*v2 + np.sqrt((4* v2**2)-8*a2*(v2*delta_t - dist_obj)))/ 2/ a2 - (2*v1 + np.sqrt((4* v1**2)-8*a1*dist_ego))/ 2/ a1 + delta_t
            return PET
        else:
            return None

    def _cal_PSD(self, x_relative_start_dist, v1, ego_decel_lon_max):
        if v1 == 0:
            return None
        else:
            if len(x_relative_start_dist) > 0:
                x_relative_start_dist0 = x_relative_start_dist.tolist()[0]
                PSD = x_relative_start_dist0 * 2 * ego_decel_lon_max / v1
                return PSD
            else:
                return None


    def velocity(self, v_x, v_y):
        v = math.sqrt(v_x ** 2 + v_y ** 2) * 3.6
        return v

    def _cal_longitudinal_safe_dist(self, v_ego_p, v_obj_p, rho, ego_accel_max, ego_decel_min, ego_decel_max):
        lon_dist_min = v_ego_p * rho + ego_accel_max * (rho ** 2) / 2 + (v_ego_p + rho * ego_accel_max) ** 2 / (
                2 * ego_decel_min) - v_obj_p ** 2 / (2 * ego_decel_max)
        return lon_dist_min

    def _cal_lateral_safe_dist(self, lat_dist, v_right, v_left, rho, a_right_lat_brake_min,
                               a_left_lat_brake_min, a_lat_max):
        # 检查除数是否为零
        if a_right_lat_brake_min == 0 or a_left_lat_brake_min == 0:
            return self._default_value('LatSD')  # 返回默认值
            
        v_right_rho = v_right + rho * a_lat_max
        v_left_rho = v_left + rho * a_lat_max
        dist_min = lat_dist + (
            (v_right + v_right_rho) * rho / 2
            + v_right_rho**2 / a_right_lat_brake_min / 2
            + ((v_left + v_right_rho) * rho / 2)
            + v_left_rho**2 / a_left_lat_brake_min / 2
        )
        return dist_min

    # DRAC (decelerate required avoid collision)
    def _cal_DRAC(self, dist, vrel_projection_in_dist, len1, len2, width1, width2, o_x1, o_x2):
        dist_length = dist - (len2 / 2 - o_x2 + len1 / 2 + o_x1)  # 4.671
        if dist_length < 0:
            dist_width = dist - (width2 / 2 + width1 / 2)
            if dist_width < 0:
                return math.inf
            else:
                d = dist_width
        else:
            d = dist_length
        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):
        BTN = (lon_a1 + lon_a - lon_v ** 2 / (2 * lon_d)) / ego_decel_lon_max  # max_ay为此车可实现的最大纵向加速度，目前为本次实例里的最大值
        return BTN

    # STN (steer threat number)
    def _cal_STN_new(self, ttc, lat_a1, lat_a, lat_d, lat_v, ego_decel_lat_max, width1, width2):
        STN = (lat_a1 + lat_a + 2 / ttc ** 2 * (lat_d + abs(ego_decel_lat_max * lat_v) * (
                width1 + width2) / 2 + abs(lat_v * ttc))) / ego_decel_lat_max
        return STN

    # BTN (brake threat number)
    def cal_BTN(self, a_y1, ay, dy, vy, max_ay):
        BTN = (a_y1 + ay - vy ** 2 / (2 * dy)) / max_ay  # max_ay为此车可实现的最大纵向加速度，目前为本次实例里的最大值
        return BTN

    # STN (steer threat number)
    def cal_STN(self, ttc, a_x1, ax, dx, vx, max_ax, width1, width2):
        STN = (a_x1 + ax + 2 / ttc ** 2 * (dx + np.sign(max_ax * vx) * (width1 + width2) / 2 + vx * ttc)) / max_ax
        return STN

    # 追尾碰撞风险
    def _normal_distribution(self, x):
        mean = 1.32
        std_dev = 0.26
        return (1 / (math.sqrt(std_dev * 2 * math.pi))) * math.exp(-0.5 * (x - mean) ** 2 / std_dev)

    def continuous_group(self, df):
        time_list = df['simTime'].values.tolist()
        frame_list = df['simFrame'].values.tolist()

        group_time = []
        group_frame = []
        sub_group_time = []
        sub_group_frame = []

        for i in range(len(frame_list)):
            if not sub_group_time or frame_list[i] - frame_list[i - 1] <= 1:
                sub_group_time.append(time_list[i])
                sub_group_frame.append(frame_list[i])
            else:
                group_time.append(sub_group_time)
                group_frame.append(sub_group_frame)
                sub_group_time = [time_list[i]]
                sub_group_frame = [frame_list[i]]

        group_time.append(sub_group_time)
        group_frame.append(sub_group_frame)
        group_time = [g for g in group_time if len(g) >= 2]
        group_frame = [g for g in group_frame if len(g) >= 2]

        # 输出图表值
        time = [[g[0], g[-1]] for g in group_time]
        frame = [[g[0], g[-1]] for g in group_frame]

        unfunc_time_df = pd.DataFrame(time, columns=['start_time', 'end_time'])
        unfunc_frame_df = pd.DataFrame(frame, columns=['start_frame', 'end_frame'])

        unfunc_df = pd.concat([unfunc_time_df, unfunc_frame_df], axis=1)
        return unfunc_df
        # 统计最危险的指标
    
    def _safe_statistic_most_dangerous(self):
        min_list = ['TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'LonSD', 'LatSD', 'TM', 'PET', 'PSD']
        max_list = ['DTC', 'BTN', 'collisionRisk', 'collisionSeverity']
        result = {}
        for metric in min_list:
            if metric in self.metric_list:
                if metric in self.df.columns:
                    val = self.df[metric].min()
                    result[metric] = float(val) if pd.notnull(val) else self._default_value(metric)
                else:
                    result[metric] = self._default_value(metric)
        for metric in max_list:
            if metric in self.metric_list:
                if metric in self.df.columns:
                    val = self.df[metric].max()
                    result[metric] = float(val) if pd.notnull(val) else self._default_value(metric)
                else:
                    result[metric] = self._default_value(metric)
        return result

    def _safe_no_obj_statistic(self):
        # 仅有自车时的默认值
        result = {metric: self._default_value(metric) for metric in self.metric_list}
        return result

    def _default_value(self, metric):
        # 统一默认值
        defaults = {
            'TTC': 10.0,
            'MTTC': 4.2,
            'THW': 2.1,
            'TLC': 10.0,
            'TTB': 10.0,
            'TM': 10.0,
            'DTC': 10.0,
            'PET': 10.0,
            'PSD': 10.0,
            'LonSD': 10.0,
            'LatSD': 2.0,
            'BTN': 1.0,
            'collisionRisk': 0.0,
            'collisionSeverity': 0.0
        }
        return defaults.get(metric, None)

    def report_statistic(self):
        if len(self.obj_id_list) == 1:
            safety_result = self._safe_no_obj_statistic()
        else:
            safety_result = self._safe_statistic_most_dangerous()
        evaluator = Score(self.data_processed.safety_config)
        result = evaluator.evaluate(safety_result)
        print("\n[安全性表现及得分情况]")
        return result

    def get_ttc_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('TTC')
        ttc_values = self.df_safe['TTC'].dropna()
        return float(ttc_values.min()) if not ttc_values.empty else self._default_value('TTC')

    def get_mttc_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('MTTC')
        mttc_values = self.df_safe['MTTC'].dropna()
        return float(mttc_values.min()) if not mttc_values.empty else self._default_value('MTTC')

    def get_thw_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('THW')
        thw_values = self.df_safe['THW'].dropna()
        return float(thw_values.min()) if not thw_values.empty else self._default_value('THW')

    def get_tlc_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('TLC')
        tlc_values = self.df_safe['TLC'].dropna()
        return float(tlc_values.min()) if not tlc_values.empty else self._default_value('TLC')

    def get_ttb_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('TTB')
        ttb_values = self.df_safe['TTB'].dropna()
        return float(ttb_values.min()) if not ttb_values.empty else self._default_value('TTB')

    def get_tm_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('TM')
        tm_values = self.df_safe['TM'].dropna()
        return float(tm_values.min()) if not tm_values.empty else self._default_value('TM')

    def get_dtc_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('DTC')
        dtc_values = self.df_safe['DTC'].dropna()
        return float(dtc_values.min()) if not dtc_values.empty else self._default_value('DTC')

    def get_pet_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('PET')
        pet_values = self.df_safe['PET'].dropna()
        return float(pet_values.min()) if not pet_values.empty else self._default_value('PET')

    def get_psd_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('PSD')
        psd_values = self.df_safe['PSD'].dropna()
        return float(psd_values.min()) if not psd_values.empty else self._default_value('PSD')

    def get_lonsd_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('LonSD')
        lonsd_values = self.df_safe['LonSD'].dropna()
        return float(lonsd_values.mean()) if not lonsd_values.empty else self._default_value('LonSD')

    def get_latsd_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('LatSD')
        latsd_values = self.df_safe['LatSD'].dropna()
        # 使用最小值而非平均值，与safety1.py保持一致
        return float(latsd_values.min()) if not latsd_values.empty else self._default_value('LatSD')

    def get_btn_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('BTN')
        btn_values = self.df_safe['BTN'].dropna()
        return float(btn_values.max()) if not btn_values.empty else self._default_value('BTN')

    def get_collision_risk_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('collisionRisk')
        risk_values = self.df_safe['collisionRisk'].dropna()
        return float(risk_values.max()) if not risk_values.empty else self._default_value('collisionRisk')

    def get_collision_severity_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('collisionSeverity')
        severity_values = self.df_safe['collisionSeverity'].dropna()
        return float(severity_values.max()) if not severity_values.empty else self._default_value('collisionSeverity')