zhaoyuan/models/safety/safety.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
##################################################################
#
# Copyright (c) 2023 CICV, Inc. All Rights Reserved
#
##################################################################
"""
@Authors:           zhanghaiwen(zhanghaiwen@china-icv.cn)
@Data:              2023/07/25
@Last Modified:     2023/07/26
@Summary:           safe metrics
"""

import sys
import os
import math
import numpy as np
import pandas as pd
from pathlib import Path 
root_path = Path(__file__).resolve().parent.parent  
sys.path.append(str(root_path)) 
from models.common.score import Score
from config import config

from collections import defaultdict
import scipy.integrate as spi
from models.common import log  # 确保这个路径是正确的，或者调整它
log_path = config.LOG_PATH
logger = log.get_logger(log_path)  


class Safe(object):
    

    def __init__(self, data_processed):
        
        self.data_processed = data_processed

        self.df = data_processed.object_df.copy()
        self.ego_df = data_processed.ego_data
        self.obj_id_list = data_processed.obj_id_list
        self.metric_list = config.SAFETY_METRIC_LIST
      
        # score data
        self.calculated_value = {  
        'TTC': 1.0,  
        'MTTC': 1.0,  
        'THW': 1.0,  
        'LonSD': 0.0,  
        'LatSD': 0.0,  
        'DRAC': 0.0,  
        'BTN': 1.0,  
        'STN': 1.0,  
        'collisionRisk': 0.0,  
        'collisionSeverity': 0.0,  
    } 

        # lists of drving control info
        
        self.time_list = data_processed.driver_ctrl_data['time_list']
        
        self.frame_list = self.ego_df['simFrame'].values.tolist()
       
        self.collisionRisk = 0
        self.empty_flag = True

        # no car following scene
        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_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._safe_param_cal_new()
    
    def _safe_param_cal_new(self):
        
        Tc = config.TC  # 安全距离
        rho = config.RHO  # 驾驶员制动反应时间
        ego_accel_max = config.EGO_ACCEL_MAX  # 自车油门最大加速度
        obj_decel_max = config.OBJ_DECEL_MAX  # 前车刹车最大减速度
        ego_decel_min = config.EGO_DECEL_MIN # 自车刹车最小减速度 ug
        ego_decel_lon_max = config.EGO_DECEL_LON_MAX
        ego_decel_lat_max = config.EGO_DECEL_LAT_MAX

        # 构建双层字典数据结构
        obj_dict = defaultdict(dict)
        # 构建字典列表，每个元素是一个字典，包含'start_time', 'end_time', 'start_frame', 'end_frame', 'type'五个键值对
        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

        # 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
            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
            a_x1 = ego_data['accelX']
            a_y1 = ego_data['accelY']
            # a1 = ego_data['accel']

            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']   # km/h to m/s
                v_y2 = obj_data['speedY']   # km/h to m/s

                v2 = obj_data['v']   # km/h to m/s
                # h2 = obj_data['posH']
                len2 = obj_data['dimX']
                width2 = obj_data['dimY']
                o_x2 = obj_data['offX']
                a_x2 = obj_data['accelX']
                a_y2 = obj_data['accelY']
                # a2 = obj_data['accel']

                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)

                obj_dict[frame_num][playerId]['lon_d'] = lon_d
                obj_dict[frame_num][playerId]['lat_d'] = lat_d


                # 代码注释，这里是筛选出车前100米，车后5米，车右边4米的目标车，用于计算安全距离
                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

                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)
                MTTC = self._cal_MTTC(TTC, vrel_projection_in_dist, arel_projection_in_dist)
                THW = self._cal_THW(dist, v_ego_p)

                # 单车道时可用
                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)

                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)
                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)
                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_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)

                obj_dict[frame_num][playerId]['lat_v_rel'] = v_x1 - v_x2
                obj_dict[frame_num][playerId]['lon_v_rel'] = v_y1 - v_y2

                # BTN = self.cal_BTN(a_y1, ay, dy, vy, max_ay)
                # STN = self.cal_STN(TTC, a_x1, ax, dx, vx, max_ax, len1, len2)

                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

                DRAC = 10 if DRAC >= 10 else DRAC
                # TTC要进行筛选，否则会出现nan或者TTC过大的情况
                if not TTC or TTC > 4000:  # threshold = 4258.41
                    collisionSeverity = 0
                    pr_death = 0
                    collisionRisk = 0
                else:
                    result, error = spi.quad(self._normal_distribution, 0, TTC - Tc)
                    collisionSeverity = 1 - result
                    pr_death = self._death_pr(obj_type, vrel_projection_in_dist)
                    collisionRisk = 0.4 * pr_death + 0.6 * collisionSeverity

                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]['LonSD'] = LonSD
                obj_dict[frame_num][playerId]['LatSD'] = LatSD
                obj_dict[frame_num][playerId]['DRAC'] = DRAC
                obj_dict[frame_num][playerId]['BTN'] = abs(BTN)
                obj_dict[frame_num][playerId]['STN'] = abs(STN)
                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', 'LonSD', 'LatSD', 'DRAC', 'BTN', 'STN', 'collisionSeverity', 'pr_death',
                    'collisionRisk']

        self.df_safe = df_safe[col_list].reset_index(drop=True)   

    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

    # 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

    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):
        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', 'LonSD', 'LatSD']
        max_list = ['DRAC', 'BTN', 'STN', 'collisionRisk', 'collisionSeverity']
        
        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()
                else:
                    self.calculated_value[metric] = 10.0

        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()
                else:
                    self.calculated_value[metric] = 10.0
        return self.calculated_value

    def _safe_no_obj_statistic(self):
        # list_metric = ["TTC","MTTC","THW","LonSD","LatSD","DRAC","BTN","STN","collisionSeverity", "collisionRisk"]
        
        return self.calculated_value
    
    def report_statistic(self, ):
        
        safety_result = {}

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