evaluate_master/custom/all_pyfiles/cicv_acc_09_overshoot_THW_new.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
##################################################################
#
# Copyright (c) 2023 CICV, Inc. All Rights Reserved
#
##################################################################
"""
@Authors:           zhanghaiwen, yangzihao
@Data:              2024/02/21
@Last Modified:     2024/02/21
@Summary:           The template of custom indicator.
"""

"""
设计思路：

"""

import math
import pandas as pd
import numpy as np
from scipy.spatial.distance import cdist  
from scipy.linalg import norm  # 用于计算向量范数 
from common import zip_time_pairs, continuous_group, get_status_active_data, _cal_THW, _cal_v_ego_projection
from log import logger

"""import functions"""


# custom metric codes
class CustomMetric(object):
    def __init__(self, all_data, case_name):
        self.data = all_data
        self.optimal_dict = self.data.config
        self.status_trigger_dict = self.data.status_trigger_dict
        self.case_name = case_name
        self.markline_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
        self.graph_list = []
        self.THW = pd.DataFrame()

        self.df = pd.DataFrame()
        self.ego_df = pd.DataFrame()
        self.df_acc = pd.DataFrame()
        self.ica_flag = False

        self.stable_average_THW = None
        self.overshoot_THW = None

        self.result = {
            "name": "跟车超调量",
            "value": [],
            # "weight": [],
            "tableData": {
                "avg": "",  # 平均值，或指标值
                "max": "",
                "min": ""
            },
            "reportData": {
                "name": "跟车超调量（%）",
                # "legend": [], # 如果有多个data，则需要增加data对应的说明，如：["横向加速度", "纵向加速度"]
                "data": [],
                "markLine": [],
                "range": [],
            },
            "statusFlag": {}
        }
        self.run()
        print(f"跟车超调量: {self.result['value']}")

    def data_extract(self):
        self.df = self.data.object_df
        self.ego_df = self.data.ego_data

        active_time_ranges = self.status_trigger_dict['ACC']['ACC_active_time']
        self.df_acc = get_status_active_data(active_time_ranges, self.df)

        active_time_ranges_ica_cruise = self.status_trigger_dict['ICA']['ICA_cruise_time']
        active_time_ranges_ica_follow = self.status_trigger_dict['ICA']['ICA_follow_time']
        if not active_time_ranges_ica_cruise and not active_time_ranges_ica_follow:
            self.ica_flag = False
        else:
            self.ica_flag = True

        if self.df_acc.empty or self.ica_flag:
            self.result['statusFlag']['function_ACC'] = False
        else:
            self.result['statusFlag']['function_ACC'] = True

    def _find_stable_THW(self, window_size, percent_deviation, set_value, distance):
        """
        在给定的数据窗口中查找稳定跟车时距离THW，并计算该段内THW的平均值。

        Args:
            window_size (int): 窗口大小，表示在数据中寻找稳定段时考虑的连续数据点数量。
            percent_deviation (float): THW值相对于设定值的允许偏差百分比。
            set_value (float): THW的设定值。

        Returns:
            None

        """
        
        # # 提取THW数组 
        # # 创建一个空的DataFrame来存储结果  
        thw_results = pd.DataFrame(columns=['simTime', 'playerId', 'THW'])  
        
        # 按时间戳分组  
        grouped = self.df_acc.groupby('simTime')  
        
        # 遍历每个时间戳分组  
        for sim_time, group in grouped:  
            # 分离出自车和其他车辆  
            ego_vehicle = group[group['playerId'] == 1]  
            other_vehicles = group[group['playerId'] != 1]  
            
            if not ego_vehicle.empty and not other_vehicles.empty:  
                # 计算位置矩阵  
                ego_positions = ego_vehicle[['posX', 'posY']].values  
                other_positions = other_vehicles[['posX', 'posY']].values  
                
                # 计算距离矩阵  
                distance_matrix = cdist(ego_positions, other_positions, 'euclidean')  
                
                # 假设自车只有一行数据（即只有一个自车实例），因此我们可以直接取第一个元素  
                ego_row = ego_vehicle.iloc[0]  
                # 找出最小距离及其索引  
                min_distance = np.min(distance_matrix)  
                min_distance_idx = np.unravel_index(np.argmin(distance_matrix), distance_matrix.shape)  
                
                # 获取最接近的车辆行  
                other_row = other_vehicles.iloc[min_distance_idx[1]]  
                # 获取最接近的车辆行  
                other_row = other_vehicles.iloc[min_distance_idx[1]]  
                
                # 计算相对位置向量  
                relative_position_vector = other_row[['posX', 'posY']].values - ego_row[['posX', 'posY']].values  
                
                # 计算自车方向向量（这里假设自车速度不为零，且方向是有效的）  
                ego_direction_vector = ego_row[['speedX', 'speedY']].values  
                ego_direction_vector_norm = norm(ego_direction_vector) 
                if ego_direction_vector_norm > 0:  
                    ego_direction_vector = ego_direction_vector / ego_direction_vector_norm  # 归一化方向向量  
                else:  
                    # 如果速度为零，则设置一个默认方向（例如，前方）  
                    ego_direction_vector = np.array([1, 0])  
                
                # 计算相对速度向量  
                
                # ego_speed = norm(ego_row[['speedX', 'speedY']].values)
                ego_speed = ego_direction_vector_norm / 3.6
                # 判断前车是否在自车的前方（基于相对位置向量和自车方向向量的点积）  
                is_ahead = np.dot(relative_position_vector, ego_direction_vector) > 0  
                
                # 如果前车在自车的前方且相对速度大于0（且距离在合理范围内），则计算THW  
                if is_ahead and min_distance < distance:  
                    thw = min_distance / ego_speed  
                    thw_results = pd.concat([thw_results, pd.DataFrame([{  
                        'simTime': sim_time,  
                        'playerId': ego_row['playerId'],  
                        'THW': thw  
                    }])], ignore_index=True)  
                else:  
                    # 如果条件不满足，可以添加一个表示“无法计算”的条目，或者忽略  
                    
                    thw = float('inf') 
        THW = thw_results['THW'].values
        self.THW = thw_results

        deviation = set_value * (percent_deviation / 100)  
        stable_start = None  
        stable_end = None  
        stable_average_THW = None

      
        
        for i in range(len(THW) - window_size + 1):
            window_data = THW[i:i + window_size]

            if all(set_value - deviation <= s <= set_value + deviation for s in window_data):
                if stable_start is None:
                    stable_start = i
                    stable_end = i + window_size - 1
                    stable_average_THW = np.mean(window_data)

                j = i + window_size
                while j < len(THW) - window_size + 1:
                    next_window_data = THW[j:j + window_size]

                    if all(set_value - deviation <= s <= set_value + deviation for s in next_window_data):
                        stable_end = j + window_size - 1
                        stable_average_THW = (stable_average_THW * (j - stable_start) + sum(next_window_data)) / (
                                j - stable_start + window_size)
                        j += window_size
                    else:
                        stable_start = j + window_size - 1
                        stable_end = i + window_size - 1
                        stable_average_THW = np.mean(window_data)
                        break 
        
        self.stable_average_THW = stable_average_THW
    def _get_first_change_index_THW(self):
        """
        获取DataFrame中'set_headway_time'列首次发生变化的索引值。

        Args:
            无参数。

        Returns:
            Union[int, None]: 如果存在变化，则返回首次发生变化的索引值（int类型），否则返回None。

        """
        change_indices = self.df_acc[self.df_acc['set_headway_time'] != self.df_acc['set_headway_time'].shift()].index  
        if not change_indices.empty:  
            # 使用首次变化的索引来获取对应的'simTime'值  
            first_change_index = change_indices.min()  
            first_change_simTime = self.df_acc.loc[first_change_index, 'simTime']  
        else:  
            first_change_simTime = None  
          
        return first_change_simTime
    def data_analyze(self):
        if self.df_acc.empty or self.ica_flag:
            self.result['value'] = [0.0]
            print(f"ACC THW overshoot_THW: 0")
        else:
            
            set_headway_time = self.df_acc['set_headway_time'].iloc[0]
           
            distance = 80.0
            self._find_stable_THW(10, 5, set_headway_time, distance)

            if set_headway_time <= 0:
                self.overshoot_THW = 0
            else:
                if not self.THW.empty:
                    if self.stable_average_THW:
                        initial_THW = self.THW['THW'].iloc[0]
                        if initial_THW > self.stable_average_THW:
                            self.overshoot_THW = (self.stable_average_THW - self.THW['THW'].min()) * 100 / self.stable_average_THW
                        elif initial_THW < self.stable_average_THW:
                            self.overshoot_THW = (self.THW['THW'].max() - self.stable_average_THW) * 100 / self.stable_average_THW
                    else:
                        self.overshoot_THW = 100
                        print("ACC THW overshoot_THW: self.stable_average_THW is None")
                else:
                    self.overshoot_THW = 0.0

            self.result['value'] = [round(self.overshoot_THW, 3)]
            print(f"ACC THW overshoot_THW: {self.overshoot_THW}")

    def markline_statistic(self):
        pass

    def report_data_statistic(self):
        # time_list = self.ego_df['simTime'].values.tolist()
        # graph_list = [x for x in self.graph_list if not np.isnan(x)]
        self.result['tableData']['avg'] = self.result['value'][0] if not self.ica_flag and not self.df_acc.empty else '-'
        self.result['tableData']['max'] = '-'
        self.result['tableData']['min'] = '-'

        # zip_vs_time = zip_time_pairs(time_list, self.graph_list)
        self.result['reportData']['data'] = []

        # self.markline_statistic()
        # markline_slices = self.markline_df.to_dict('records')
        self.result['reportData']['markLine'] = []

        self.result['reportData']['range'] = [0, 1.2]

    def run(self):
        # logger.info(f"Custom metric run:[{self.result['name']}].")
        logger.info(f"[case:{self.case_name}] Custom metric:[overshoot_THW:{self.result['name']}] evaluate.")

        try:
            self.data_extract()
        except Exception as e:
            logger.error(f"[case:{self.case_name}] Custom metric:{self.result['name']} data extract ERROR!", e)

        try:
            self.data_analyze()
        except Exception as e:
            logger.error(f"[case:{self.case_name}] Custom metric:{self.result['name']} data analyze ERROR!", e)

        try:
            self.report_data_statistic()
        except Exception as e:
            logger.error(f"[case:{self.case_name}] Custom metric:{self.result['name']} report data statistic ERROR!", e)

# if __name__ == "__main__":
#     pass