#!/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