zhaoyuan/modules/metric/safety.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
安全指标计算模块（支持多目标物） - 场景感知版本
优化要点：
1. 重构代码结构，提高可读性
2. 去除冗余代码，优化性能
3. 保持原始计算逻辑不变
4. 增加全面注释
5. 优化数据处理流程
"""

import os
import numpy as np
import pandas as pd
import math
import scipy.integrate as spi
from collections import defaultdict
from typing import Dict, Any, List, Optional, Tuple
from pathlib import Path

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


# ==================== 安全指标计算接口函数 ====================
# 每个函数对应一个安全指标的计算入口
# ------------------------------------------------------------

def calculate_ttc(data_processed, plot_path) -> dict:
    """计算碰撞时间(TTC)"""
    return _calculate_metric('TTC', data_processed, plot_path)


def calculate_mttc(data_processed, plot_path) -> dict:
    """计算修正碰撞时间(MTTC)"""
    return _calculate_metric('MTTC', data_processed, plot_path)


def calculate_thw(data_processed, plot_path) -> dict:
    """计算车头时距(THW)"""
    return _calculate_metric('THW', data_processed, plot_path)


def calculate_ttb(data_processed, plot_path) -> dict:
    """计算制动时间(TTB)"""
    return _calculate_metric('TTB', data_processed, plot_path)


def calculate_tm(data_processed, plot_path) -> dict:
    """计算时间裕度(TM)"""
    return _calculate_metric('TM', data_processed, plot_path)


def calculate_dtc(data_processed, plot_path) -> dict:
    """计算碰撞距离(DTC)"""
    return _calculate_metric('DTC', data_processed, plot_path)


def calculate_psd(data_processed, plot_path) -> dict:
    """计算预测安全距离比(PSD)"""
    return _calculate_metric('PSD', data_processed, plot_path)


def calculate_collisionrisk(data_processed, plot_path) -> dict:
    """计算碰撞风险(collisionRisk)"""
    return _calculate_metric('collisionRisk', data_processed, plot_path)


def calculate_lonsd(data_processed, plot_path) -> dict:
    """计算纵向安全距离(LonSD)"""
    return _calculate_metric('LonSD', data_processed, plot_path)


def calculate_latsd(data_processed, plot_path) -> dict:
    """计算横向安全距离(LatSD)"""
    return _calculate_metric('LatSD', data_processed, plot_path)


def calculate_btn(data_processed, plot_path) -> dict:
    """计算制动威胁数(BTN)"""
    return _calculate_metric('BTN', data_processed, plot_path)


def calculate_collisionseverity(data_processed, plot_path) -> dict:
    """计算碰撞严重性(collisionSeverity)"""
    return _calculate_metric('collisionSeverity', data_processed, plot_path)


def _calculate_metric(metric_name, data_processed, plot_path) -> dict:
    """安全指标计算的通用处理函数"""
    key_name = metric_name
    result_key = {metric_name: None}

    # 检查输入数据有效性
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return result_key

    try:
        safety = SafetyCalculator(data_processed)

        # 特殊处理 collisionRisk 和 collisionSeverity 指标
        if metric_name.lower() == 'collisionrisk':
            metric_value = safety.get_collision_risk_value()
        elif metric_name.lower() == 'collisionseverity':
            metric_value = safety.get_collision_severity_value()
        else:
            # 其他指标使用原有逻辑
            metric_value = getattr(safety, f'get_{metric_name.lower()}_value')()

        # 生成图表数据
        metric_data = getattr(safety, f'{metric_name.lower()}_data', None)
        if metric_data:
            safety.generate_metric_chart(metric_name, plot_path)

        LogManager().get_logger().info(f"安全指标[{metric_name}]计算结果: {metric_value}")
        return {metric_name: metric_value}
    except Exception as e:
        LogManager().get_logger().error(f"{metric_name}计算异常: {str(e)}", exc_info=True)
        return result_key


# ==================== 安全指标注册与批处理类 ====================
# 用于根据配置批量执行安全指标计算
# ------------------------------------------------------------

class SafetyRegistry:
    """安全指标注册器，根据配置动态注册和执行指标计算"""

    def __init__(self, data_processed, plot_path):
        self.logger = LogManager().get_logger()
        self.data = data_processed
        self.plot_path = plot_path

        # 检查安全配置
        if not hasattr(data_processed, 'safety_config') or not data_processed.safety_config:
            self.logger.warning("安全配置为空，跳过安全指标计算")
            self.safety_config = {}
            self.metrics = []
            self._registry = {}
            return

        self.safety_config = data_processed.safety_config.get("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 = {}
        if not hasattr(self, 'safety_config') or not self.safety_config or not self._registry:
            self.logger.info("安全配置为空或无注册指标，返回空结果")
            return results

        for name, func in self._registry.items():
            try:
                result = func(self.data, self.plot_path)
                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, plot_path):
        self.data = data_processed
        self.logger = LogManager().get_logger()
        self.plot_path = plot_path

        # 初始化安全指标注册器
        if not hasattr(data_processed, 'safety_config') or not data_processed.safety_config:
            self.logger.warning("安全配置为空，跳过安全指标计算初始化")
            self.registry = None
        else:
            self.registry = SafetyRegistry(self.data, self.plot_path)

    def report_statistic(self):
        """生成安全指标统计报告"""
        if self.registry is None:
            self.logger.info("安全指标管理器未初始化，返回空结果")
            return {}
        return self.registry.batch_execute()


# ==================== 安全指标计算核心类 ====================
# 包含所有安全指标的具体计算实现
# ------------------------------------------------------------

class SafetyCalculator:
    """安全指标计算器，实现各种安全指标的具体计算"""

    DEFAULT_VALUES = {
        'TTC': 10.0, 'MTTC': 3.3, 'THW': 2.5,
        'TTB': 10.0, 'TM': 10.0, 'DTC': 10.0, 'PSD': 10.0,
        'LonSD': 100.0, 'LatSD': 2.0, 'BTN': 0.0,
        'collisionRisk': 0.0, 'collisionSeverity': 0.0
    }

    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()
        self.obj_id_list = data_processed.obj_id_list

        # 初始化配置参数
        self._init_config_params()

        # 初始化数据结构和指标存储
        self._init_data_structures()

        # 执行安全参数计算
        if self.obj_id_list:
            self.logger.info("开始执行安全参数计算，目标物数量: %d", len(self.obj_id_list) - 1)
            self._calculate_safety_parameters()
            self.logger.info("安全参数计算完成")
        else:
            self.logger.info("没有目标物，跳过安全参数计算")
            self.empty_flag = True

    def _init_config_params(self):
        """从配置中初始化车辆参数"""
        config = self.data_processed.vehicle_config
        self.rho = config.get("RHO", 1.5)  # 默认反应时间1.5秒
        self.ego_accel_max = config.get("EGO_ACCEL_MAX", 3.0)  # 默认最大加速度3.0 m/s²
        self.obj_decel_max = config.get("OBJ_DECEL_MAX", 3.0)  # 默认目标最大减速度3.0 m/s²
        self.ego_decel_min = config.get("EGO_DECEL_MIN", 4.0)  # 默认自车最小减速度4.0 m/s²
        self.ego_decel_lon_max = config.get("EGO_DECEL_LON_MAX", 4.0)  # 默认纵向最大减速度4.0 m/s²
        self.ego_decel_lat_max = config.get("EGO_DECEL_LAT_MAX", 3.0)  # 默认横向最大减速度3.0 m/s²
        self.ego_width = config.get("CAR_WIDTH", 2.0)  # 默认车辆宽度2.0米
        self.ego_length = config.get("CAR_LENGTH", 4.5)  # 默认车辆长度4.5米
        self.vehicle_length = config.get("VEHICLE_LENGTH", 4.5)  # 默认车辆长度4.5米
        self.vehicle_width = config.get("VEHICLE_WIDTH", 2.0)  # 默认车辆宽度2.0米
        self.pedestrian_length = config.get("PEDESTRIAN_LENGTH", 0.5)  # 默认行人长度0.5米
        self.pedestrian_width = config.get("PEDESTRIAN_WIDTH", 0.5)  # 默认行人宽度0.5米

        # 计算派生参数
        self.max_deceleration = self.ego_decel_lon_max
        self.ego_decel_max = np.sqrt(self.ego_decel_lon_max ** 2 + self.ego_decel_lat_max ** 2)
        self.ped_width = self.pedestrian_width
        self.ped_length = self.pedestrian_length

    def _init_data_structures(self):
        """初始化数据存储结构"""
        self.time_list = self.ego_df['simTime'].values.tolist()
        self.frame_list = self.ego_df['simFrame'].values.tolist()
        self.empty_flag = True  # 标记是否有有效目标

        # 初始化指标数据存储
        self._init_metric_storage()

        # 创建输出目录
        self.output_dir = os.path.join(os.getcwd(), 'data')
        os.makedirs(self.output_dir, exist_ok=True)

    def _init_metric_storage(self):
        """初始化所有指标的存储列表"""
        metrics = [
            'ttc', 'mttc', 'thw', 'ttb', 'tm', 'lonsd', 'latsd', 'btn',
            'collision_risk', 'collision_severity'
        ]
        for metric in metrics:
            setattr(self, f'{metric}_data', [])

    def _calculate_safety_parameters(self):
        """核心安全参数计算方法"""
        # 预处理：构建按帧组织的目标字典
        obj_dict = self._preprocess_object_data()
        df_list = []
        EGO_PLAYER_ID = 1
        found_valid_target = False

        # 逐帧处理数据
        for frame_num in self.frame_list:
            try:
                ego_data = obj_dict[frame_num][EGO_PLAYER_ID]
            except KeyError:
                continue

            frame_targets = self._process_frame_targets(frame_num, ego_data, obj_dict, EGO_PLAYER_ID)

            if frame_targets:
                found_valid_target = True
                df_fnum = pd.DataFrame(frame_targets)
                df_list.append(df_fnum)

        # 合并处理结果
        self._postprocess_results(df_list, found_valid_target)

    def _preprocess_object_data(self):
        """预处理目标数据为按帧组织的字典"""
        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
        return obj_dict

    def _process_frame_targets(self, frame_num, ego_data, obj_dict, ego_id):
        """处理单帧内的所有目标物"""
        frame_targets = []
        vx_ego = ego_data.get('lon_v_vehicle', 0)
        vy_ego = ego_data.get('lat_v_vehicle', 0)
        self.lane_width = ego_data.get('lane_width', 3.75)

        for player_id in self.obj_id_list:
            if player_id == ego_id:
                continue

            try:
                obj_data = obj_dict[frame_num][player_id]
            except KeyError:
                continue

            # 计算目标物安全指标
            result = self._calculate_target_metrics(ego_data, obj_data, vx_ego, vy_ego)
            obj_data.update(result)

            frame_targets.append(obj_data)

        return frame_targets

    def _calculate_target_metrics(self, ego_data, obj_data, vx_ego, vy_ego):
        """计算单个目标物的安全指标"""
        # 提取基本参数
        obj_type = obj_data.get('type', 0)
        lon_d = obj_data.get('x_relative_start_dist', 0)
        lat_d = obj_data.get('y_relative_start_dist', 0)
        vx_obj = obj_data.get('lon_v_vehicle', 0)
        vy_obj = obj_data.get('lat_v_vehicle', 0)

        # 计算相对运动参数
        vx_rel = vx_obj - vx_ego
        vy_rel = vy_obj - vy_ego
        dist = math.sqrt(lon_d ** 2 + lat_d ** 2)

        # 计算航向角差异
        h1 = ego_data['posH']
        h2 = obj_data.get('posH', h1)
        heading_diff = abs(h1 - h2)
        if heading_diff > 180:
            heading_diff = 360 - heading_diff

        # 计算相对加速度
        ax_ego = ego_data.get('lon_acc_vehicle', 0)
        ay_ego = ego_data.get('lat_acc_vehicle', 0)
        ax_obj = obj_data.get('lon_acc_vehicle', 0)
        ay_obj = obj_data.get('lat_acc_vehicle', 0)
        ax_rel_ego = ax_obj - ax_ego
        ay_rel_ego = ay_obj - ay_ego

        # 计算投影
        vrel_projection = self._cal_v_projection_using_relative(lon_d, lat_d, vx_rel, vy_rel)
        accl_projection = self._cal_a_projection_using_relative(lon_d, lat_d, ax_rel_ego, ay_rel_ego)

        # 检查目标相关性
        is_relevant = self._is_relevant_target(lat_d, lon_d, vy_rel, heading_diff, obj_type)

        # 检查过滤条件
        passed_conflict = self._has_passed_conflict_point(lon_d, lat_d, obj_type, heading_diff)
        moving_away = self._is_moving_away(
            (vx_ego, vy_ego), (vx_obj, vy_obj), lon_d, lat_d, obj_type, heading_diff
        )
        skip_risk_calculation = passed_conflict and (moving_away or obj_data.get('v', 0) < 0.05)

        # 计算指标
        if not is_relevant or skip_risk_calculation:
            return self._get_default_metrics(obj_type)
        else:
            return self._calculate_relevant_metrics(
                dist, vrel_projection, accl_projection,
                lon_d, lat_d, vx_ego, vy_ego, vx_rel, ax_rel_ego,
                heading_diff, obj_type, ego_data, obj_data
            )

    def _get_default_metrics(self, obj_type):
        """获取默认指标值"""
        return {
            'TTC': self.DEFAULT_VALUES['TTC'],
            'MTTC': self.DEFAULT_VALUES['MTTC'],
            'THW': self.DEFAULT_VALUES['THW'] if obj_type != 5 else None,
            'TTB': self.DEFAULT_VALUES['TTB'] if obj_type != 5 else None,
            'TM': self.DEFAULT_VALUES['TM'] if obj_type != 5 else None,
            'PSD': self.DEFAULT_VALUES['PSD'],
            'DTC': self.DEFAULT_VALUES['DTC'],
            'LonSD': self.DEFAULT_VALUES['LonSD'] if obj_type != 5 else None,
            'LatSD': self.DEFAULT_VALUES['LatSD'] if obj_type != 5 else None,
            'BTN': self.DEFAULT_VALUES['BTN'] if obj_type != 5 else None,
            'collisionSeverity': self.DEFAULT_VALUES['collisionSeverity'],
            'pr_death': 0,
            'collisionRisk': self.DEFAULT_VALUES['collisionRisk']
        }

    def _calculate_relevant_metrics(self, dist, vrel_projection, accl_projection,
                                    lon_d, lat_d, vx_ego, vy_ego, vx_rel, ax_rel_ego,
                                    heading_diff, obj_type, ego_data, obj_data):
        """计算相关目标的指标"""
        Tc = 0.3  # 反应时间

        # 核心指标计算
        metrics = {
            'TTC': self._cal_TTC(dist, vrel_projection),
            'MTTC': self._cal_MTTC(dist, vrel_projection, ego_data.get('lon_acc_vehicle', 0)),
            'THW': self._cal_THW(lon_d, vx_ego),
            'TTB': self._cal_TTB(dist, vrel_projection, self.ego_decel_max) if obj_type != 5 else None,
            'TM': self._cal_TM(vrel_projection, obj_data.get('v', 0),
                               obj_data.get('lon_acc_vehicle', 0),
                               ego_data.get('v', 0),
                               ego_data.get('lon_acc_vehicle', 0)) if obj_type != 5 else None,
            'PSD': self._cal_PSD(dist, vrel_projection, self.max_deceleration,
                                 heading_diff, vx_ego, vy_ego, obj_type, lon_d, lat_d),
            'DTC': self._cal_DTC(vx_rel, ax_rel_ego, self.rho) if abs(heading_diff) < 30 else self.DEFAULT_VALUES[
                'DTC'],
            'LonSD': self._cal_longitudinal_safe_dist(vx_ego, vx_rel, self.rho,
                                                      self.ego_decel_min,
                                                      self.obj_decel_max) if obj_type != 5 else None,
            'LatSD': self._cal_lateral_safe_dist(abs(lat_d), vy_ego, self.ego_width,
                                                 self.lane_width, self.ego_decel_lat_max) if obj_type != 5 else None,
            'BTN': self._cal_BTN_new(ego_data.get('lon_acc_vehicle', 0),
                                     ego_data.get('lon_acc_vehicle', 0),
                                     abs(lon_d), vx_ego, self.ego_decel_lon_max) if obj_type != 5 else None
        }

        # 处理碰撞风险相关指标
        if metrics['TTC'] is None or metrics['TTC'] > 4000:
            metrics.update({
                'collisionSeverity': 0,
                'pr_death': 0,
                'collisionRisk': 0
            })
        else:
            try:
                result, _ = spi.quad(self._normal_distribution, 0, metrics['TTC'] - Tc)
                collisionSeverity = 1 - result
                pr_death = self._death_pr(obj_type, vrel_projection)
                collisionRisk = self._cal_collisionRisk_level(obj_type, vrel_projection, collisionSeverity)
                metrics.update({
                    'collisionSeverity': collisionSeverity * 100,
                    'pr_death': pr_death * 100,
                    'collisionRisk': collisionRisk * 100
                })
            except Exception as e:
                self.logger.error(f"碰撞风险计算错误: {e}")
                metrics.update({
                    'collisionSeverity': 0,
                    'pr_death': 0,
                    'collisionRisk': 0
                })

        return metrics

    def _postprocess_results(self, df_list, found_valid_target):
        """后处理计算结果"""
        self.empty_flag = not found_valid_target

        # 合并所有帧数据
        if df_list:
            self.df_safe = pd.concat(df_list)
            col_list = ['simTime', 'simFrame', 'playerId',
                        'TTC', 'MTTC', 'THW', 'TTB', 'TM', 'DTC', 'PSD', 'LonSD', 'LatSD', 'BTN',
                        'collisionSeverity', 'pr_death', 'collisionRisk']
            self.df_safe = self.df_safe[col_list].reset_index(drop=True)
        else:
            self.df_safe = pd.DataFrame()
            self.empty_flag = True

        self.logger.info(f"处理完成，找到有效目标: {not self.empty_flag}")

    # ==================== 核心计算方法 ====================

    def _cal_v_projection_using_relative(self, lon_d, lat_d, vx_rel, vy_rel):
        """计算速度投影（自车坐标系）"""
        dist = math.sqrt(lon_d ** 2 + lat_d ** 2)
        if dist < 1e-6:
            return 0.0
        U_ABx = lon_d / dist
        U_ABy = lat_d / dist
        return vx_rel * U_ABx + vy_rel * U_ABy

    def _cal_a_projection_using_relative(self, lon_d, lat_d, ax_rel, ay_rel):
        """计算加速度投影（自车坐标系）"""
        dist = math.sqrt(lon_d ** 2 + lat_d ** 2)
        if dist < 1e-6:
            return 0.0
        U_ABx = lon_d / dist
        U_ABy = lat_d / dist
        return ax_rel * U_ABx + ay_rel * U_ABy

    def _cal_TTC(self, dist, vrel_projection):
        """计算碰撞时间(TTC)"""
        if vrel_projection >= 0:  # 不接近
            return None
        return dist / abs(vrel_projection)

    def _cal_MTTC(self, dist, vrel_projection, a_ego):
        """计算修正的碰撞时间(MTTC)"""
        if vrel_projection >= 0 or dist <= 0 or a_ego >= 0:
            return None

        try:
            if abs(a_ego) < 1e-6:
                return dist / abs(vrel_projection) if abs(vrel_projection) > 1e-6 else None

            discriminant = vrel_projection ** 2 + 2 * a_ego * dist
            if discriminant < 0:
                return None

            t1 = (-vrel_projection + math.sqrt(discriminant)) / a_ego
            t2 = (-vrel_projection - math.sqrt(discriminant)) / a_ego

            valid_times = [t for t in (t1, t2) if t > 0]
            return min(valid_times) if valid_times else None
        except Exception as e:
            self.logger.warning(f"MTTC计算错误: {e}")
            return None

    def _cal_THW(self, lon_d, vx_ego):
        """计算车头时距(THW)"""
        if vx_ego is None or vx_ego <= 0 or lon_d is None or lon_d <= 0:
            return None
        return lon_d / vx_ego

    def _cal_TTB(self, dist, vrel_projection, ego_decel_max):
        """计算制动时间(TTB)"""
        if vrel_projection is None or ego_decel_max is None or vrel_projection >= 0:
            return None

        try:
            TTB = (dist + vrel_projection ** 2 / (2 * ego_decel_max) / vrel_projection)
            return TTB if TTB > 0 and not (np.isinf(TTB) or np.isnan(TTB)) else None
        except Exception as e:
            self.logger.warning(f"计算TTB时出错: {e}")
            return None

    def _cal_TM(self, vrel_projection, v2, a2, v1, a1):
        """计算时间裕度(TM)"""
        if (vrel_projection is None or v2 is None or a2 is None or a2 <= 0 or
                v1 is None or a1 is None or a1 <= 0 or vrel_projection >= 0):
            return None

        try:
            TM = (v2 ** 2 / (2 * a2) - v1 ** 2 / (2 * a1)) / v1
            return TM if TM > 0 and not (np.isinf(TM) or np.isnan(TM)) else None
        except Exception as e:
            self.logger.warning(f"计算TM时出错: {e}")
            return None

    def _cal_PSD(self, dist, vrel_projection, max_decel, heading_diff, vx_ego, vy_ego, obj_type, relative_x,
                 relative_y):
        """计算预测安全距离比(PSD)"""
        abs_heading_diff = abs(heading_diff)

        # 根据航向差确定场景类型
        if abs_heading_diff < 45:
            # 纵向场景
            D = abs(relative_x)
            v_dir = abs(vx_ego)
            min_stopping_dist = (v_dir ** 2) / (2 * self.ego_decel_lon_max)
        elif 45 <= abs_heading_diff <= 135:
            # 横向场景
            D = abs(relative_y)
            v_dir = abs(vy_ego)
            if obj_type == 5:  # 行人
                reaction_dist = abs(relative_y) * self.rho
                braking_dist = (v_dir ** 2) / (2 * self.ego_decel_lat_max)
                min_stopping_dist = reaction_dist + braking_dist
            else:  # 车辆
                min_stopping_dist = (v_dir ** 2) / (2 * self.ego_decel_lat_max)
        else:  # 对向场景
            return self.DEFAULT_VALUES['PSD']

        # 处理特殊情况
        if min_stopping_dist < 1e-3:
            return float('inf') if D > 0.1 else 0.0

        return D / min_stopping_dist

    def _cal_DTC(self, vx_rel, ax_rel_ego, t):
        """计算碰撞距离(DTC)"""
        if vx_rel >= 0:  # 没有接近风险
            return self.DEFAULT_VALUES['DTC']
        try:
            speed = abs(vx_rel)
            reaction_distance = speed * t
            braking_distance = (speed ** 2) / (2 * self.ego_decel_lon_max)
            dtc = reaction_distance + braking_distance
            return max(0, dtc)
        except Exception as e:
            self.logger.warning(f"DTC计算错误: {e}")
            return self.DEFAULT_VALUES['DTC']

    def _cal_longitudinal_safe_dist(self, v_ego, v_lead_rel, rho, decel_ego, decel_lead):
        """计算纵向安全距离(LonSD)"""
        try:
            v_lead = v_ego + v_lead_rel
            reaction_distance = v_ego * rho
            braking_distance_ego = v_ego ** 2 / (2 * decel_ego) if v_ego > 0 else 0
            braking_distance_lead = v_lead ** 2 / (2 * decel_lead) if v_lead > 0 else 0
            safe_dist = reaction_distance + braking_distance_ego - braking_distance_lead
            return max(2.0, safe_dist)
        except Exception as e:
            self.logger.warning(f"计算纵向安全距离错误: {e}")
            return 25.0

    def _cal_lateral_safe_dist(self, lat_dist, v_ego_lat, vehicle_width, lane_width, decel_lat_max):
        """计算横向安全距离(LatSD)"""
        try:
            available_space = lane_width - vehicle_width
            base_margin = 0.5
            speed_margin = 0.1 * abs(v_ego_lat)
            safe_dist = min(base_margin + speed_margin, available_space / 2)
            return max(base_margin, safe_dist)
        except Exception as e:
            self.logger.warning(f"计算横向安全距离错误: {e}")
            return 1.0

    def _cal_BTN_new(self, lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max):
        """计算制动威胁数(BTN)"""
        if lon_d is None or lon_d <= 0 or ego_decel_lon_max is None or ego_decel_lon_max <= 0:
            return None
        try:
            return (lon_a1 + lon_a - (lon_v ** 2) / (2 * lon_d)) / ego_decel_lon_max
        except:
            return None

    def _death_pr(self, obj_type, v_relative):
        """计算死亡概率"""
        if obj_type == 5:  # 行人
            return 1 / (1 + np.exp(7.723 - 0.15 * v_relative))
        else:  # 车辆
            return 1 / (1 + np.exp(8.192 - 0.12 * v_relative))

    def _cal_collisionRisk_level(self, obj_type, v_relative, collisionSeverity):
        """计算碰撞风险等级"""
        p_death = self._death_pr(obj_type, v_relative)
        return 0.4 * p_death + 0.6 * collisionSeverity

    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 _is_relevant_target(self, lat_d, lon_d, v_lat, heading_diff, obj_type):
        """判断目标物是否相关"""
        abs_lat_d = abs(lat_d)
        abs_heading_diff = abs(heading_diff)
        lane_half_width = self.lane_width / 2
        ego_half_width = self.ego_width / 2
        ped_half_width = self.ped_width / 2
        safe_width = ego_half_width + ped_half_width + 0.2

        # 行人目标处理
        if obj_type == 5:
            if abs_heading_diff >= 60:  # 横穿行人
                return (0 < lon_d < 50) and (abs_lat_d < safe_width or abs_lat_d < lane_half_width)
            else:  # 沿路行人
                return (0 < lon_d < 30) and (abs_lat_d < safe_width) and abs(v_lat) < 1.0

        # 车辆类目标处理
        if abs_heading_diff < 45:  # 纵向目标
            return abs_lat_d < lane_half_width + 0.1 and abs(lon_d) < 80

        if 45 <= abs_heading_diff <= 135:  # 横向目标
            return 0 < lon_d < 30 and abs_lat_d < ego_half_width + 0.1

        return False  # 反向目标

    def _has_passed_conflict_point(self, relative_x, relative_y, obj_type, heading_diff):
        """判断目标是否已越过冲突点"""
        if obj_type == 5:  # 行人
            obj_length = self.pedestrian_length
            obj_width = self.pedestrian_width
        else:  # 车辆
            obj_length = self.vehicle_length
            obj_width = self.vehicle_width

        length_safe_dist = (self.ego_length + obj_length) / 2
        width_safe_dist = (self.ego_width + obj_width) / 2

        if obj_type == 5:
            return relative_x < -length_safe_dist or abs(relative_y) > width_safe_dist * 2
        else:
            if abs(heading_diff) < 45:  # 同向
                return relative_x < -length_safe_dist or abs(relative_y) > width_safe_dist * 1.5
            elif 45 <= abs(heading_diff) <= 135:  # 横向
                return relative_x < -length_safe_dist or relative_x > length_safe_dist
            else:  # 对向
                return ((relative_x < -length_safe_dist and relative_y < -width_safe_dist) or
                        (relative_x < -length_safe_dist and relative_y > width_safe_dist) or
                        (relative_x > length_safe_dist and relative_y < -width_safe_dist) or
                        (relative_x > length_safe_dist and relative_y > width_safe_dist))

    def _is_moving_away(self, ego_vel, target_vel, relative_x, relative_y, obj_type, heading_diff):
        """判断目标是否正在远离"""
        vx_rel = target_vel[0] - ego_vel[0]
        vy_rel = target_vel[1] - ego_vel[1]

        dist = math.sqrt(relative_x ** 2 + relative_y ** 2)
        if dist < 1e-6:
            return False

        v_projection = (vx_rel * relative_x + vy_rel * relative_y) / dist

        if obj_type == 5:  # 行人
            if relative_x < 0:  # 后方
                return v_projection > 0.5
            else:  # 前方
                return v_projection < -0.5
        else:
            abs_heading_diff = abs(heading_diff)

            if abs_heading_diff < 45:  # 同向
                if relative_x < 0:  # 后方
                    return v_projection > 0
                else:  # 前方
                    return v_projection < 0
            elif 45 <= abs_heading_diff <= 135:  # 横向
                if relative_x > 0:  # 前方
                    return v_projection < 0
                else:  # 后方
                    return v_projection > 0
            else:  # 对向
                return False

    # ==================== 指标获取方法 ====================

    def _get_metric_value(self, metric_name, aggregation='min'):
        """通用指标值获取方法"""
        if self.empty_flag or self.df_safe is None or self.df_safe.empty:
            return self.DEFAULT_VALUES.get(metric_name, None)

        values = self.df_safe[metric_name].dropna()
        if values.empty:
            return self.DEFAULT_VALUES.get(metric_name, None)

        # 根据聚合类型获取值
        if aggregation == 'min':
            metric_value = float(values.min())
        elif aggregation == 'max':
            metric_value = float(values.max())
        elif aggregation == 'mean':
            metric_value = float(values.mean())
        else:
            metric_value = float(values.min())

        # 存储指标数据
        metric_data = []
        for _, row in self.df_safe.iterrows():
            if pd.notnull(row[metric_name]):
                metric_data.append({
                    'simTime': row['simTime'],
                    'simFrame': row['simFrame'],
                    'playerId': row['playerId'],
                    metric_name: row[metric_name]
                })
        setattr(self, f'{metric_name.lower()}_data', metric_data)

        return metric_value

    def get_ttc_value(self) -> float:
        return self._get_metric_value('TTC')

    def get_mttc_value(self) -> float:
        return self._get_metric_value('MTTC')

    def get_thw_value(self) -> float:
        return self._get_metric_value('THW')

    def get_ttb_value(self) -> float:
        return self._get_metric_value('TTB')

    def get_tm_value(self) -> float:
        return self._get_metric_value('TM')

    def get_dtc_value(self) -> float:
        return self._get_metric_value('DTC', 'min')

    def get_psd_value(self) -> float:
        return self._get_metric_value('PSD', 'min')

    def get_lonsd_value(self) -> float:
        return self._get_metric_value('LonSD', 'mean')

    def get_latsd_value(self) -> float:
        return self._get_metric_value('LatSD', 'min')

    def get_btn_value(self) -> float:
        return self._get_metric_value('BTN', 'max')

    def get_collision_risk_value(self) -> float:
        return self._get_metric_value('collisionRisk', 'max')

    def get_collision_severity_value(self) -> float:
        return self._get_metric_value('collisionSeverity', 'max')

    # ==================== 辅助方法 ====================

    def generate_metric_chart(self, metric_name: str, plot_path: Path) -> None:
        """生成指标图表"""
        try:
            self.output_dir = plot_path if plot_path else os.path.join(os.getcwd(), 'data')
            os.makedirs(self.output_dir, exist_ok=True)

            chart_path = generate_safety_chart_data(self, metric_name, self.output_dir)
            if chart_path:
                self.logger.info(f"{metric_name}图表已生成: {chart_path}")
            else:
                self.logger.warning(f"{metric_name}图表生成失败")
        except Exception as e:
            self.logger.error(f"生成{metric_name}图表失败: {str(e)}", exc_info=True)