#!/usr/bin/env python # -*- coding: utf-8 -*- ################################################################## # # Copyright (c) 2023 CICV, Inc. All Rights Reserved # ################################################################## """ @Authors: zhanghaiwen(zhanghaiwen@china-icv.cn), yangzihao(yangzihao@china-icv.cn) @Data: 2023/06/25 @Last Modified: 2023/06/25 @Summary: Comfort metrics """ import sys import math import pandas as pd import numpy as np import scipy.signal from pathlib import Path from modules.lib.score import Score from modules.lib.common import get_interpolation, get_frame_with_time from modules.config import config from modules.lib import data_process from modules.lib.log_manager import LogManager def peak_valley_decorator(method): def wrapper(self, *args, **kwargs): peak_valley = self._peak_valley_determination(self.df) pv_list = self.df.loc[peak_valley, ['simTime', 'speedH']].values.tolist() if len(pv_list) != 0: flag = True p_last = pv_list[0] for i in range(1, len(pv_list)): p_curr = pv_list[i] if self._peak_valley_judgment(p_last, p_curr): # method(self, p_curr, p_last) method(self, p_curr, p_last, flag, *args, **kwargs) else: p_last = p_curr return method else: flag = False p_curr = [0, 0] p_last = [0, 0] method(self, p_curr, p_last, flag, *args, **kwargs) return method return wrapper class Comfort(object): """ Class for achieving comfort metrics for autonomous driving. Attributes: dataframe: Vehicle driving data, stored in dataframe format. """ def __init__(self, data_processed): # self.logger = log.get_logger() self.eval_data = pd.DataFrame() self.data_processed = data_processed self.logger = LogManager().get_logger() # 获取全局日志实例 self.data = data_processed.ego_data # self.mileage = data_processed.report_info['mileage'] self.ego_df = pd.DataFrame() self.discomfort_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type']) self.calculated_value = { 'Weaving': 0, 'shake': 0, 'cadence': 0, 'slamBrake': 0, 'slamAccelerate': 0, } # self.time_list = data_processed.driver_ctrl_data['time_list'] # self.frame_list = data_processed.driver_ctrl_data['frame_list'] self.time_list = self.data['simTime'].values.tolist() self.frame_list = self.data['simFrame'].values.tolist() self.count_dict = {} self.duration_dict = {} self.strength_dict = {} self.discomfort_count = 0 self.zigzag_count = 0 self.shake_count = 0 self.cadence_count = 0 self.slam_brake_count = 0 self.slam_accel_count = 0 self.zigzag_strength = 0 self.shake_strength = 0 self.cadence_strength = 0 self.slam_brake_strength = 0 self.slam_accel_strength = 0 self.discomfort_duration = 0 self.zigzag_duration = 0 self.shake_duration = 0 self.cadence_duration = 0 self.slam_brake_duration = 0 self.slam_accel_duration = 0 self.zigzag_time_list = [] self.zigzag_frame_list = [] self.zigzag_stre_list = [] self.cur_ego_path_list = [] self.curvature_list = [] self._get_data() self._comf_param_cal() def _get_data(self): """ """ self.ego_df = self.data[config.COMFORT_INFO].copy() self.df = self.ego_df.reset_index(drop=True) # 索引是csv原索引 # def _cal_cur_ego_path(self, row): # try: # divide = (row['speedX'] ** 2 + row['speedY'] ** 2) ** (3 / 2) # if not divide: # res = None # else: # res = (row['speedX'] * row['accelY'] - row['speedY'] * row['accelX']) / divide # except: # res = None # return res import numpy as np def _cal_cur_ego_path(self, row): try: # 计算速度平方和，判断是否接近零 speed_sq = row['speedX']**2 + row['speedY']**2 if speed_sq < 1e-6: # 阈值根据实际场景调整 return 1e5 # 速度接近零时返回极大曲率 divide = speed_sq ** (3/2) res = (row['speedX'] * row['accelY'] - row['speedY'] * row['accelX']) / divide return res except Exception as e: return 1e5 # 异常时也返回极大值（如除零、缺失值等） def _comf_param_cal(self): """ """ # [log] self.ego_df['ip_acc'] = self.ego_df['v'].apply(get_interpolation, point1=[18, 4], point2=[72, 2]) self.ego_df['ip_dec'] = self.ego_df['v'].apply(get_interpolation, point1=[18, -5], point2=[72, -3.5]) self.ego_df['slam_brake'] = (self.ego_df['lon_acc'] - self.ego_df['ip_dec']).apply( lambda x: 1 if x < 0 else 0) self.ego_df['slam_accel'] = (self.ego_df['lon_acc'] - self.ego_df['ip_acc']).apply( lambda x: 1 if x > 0 else 0) self.ego_df['cadence'] = self.ego_df.apply( lambda row: self._cadence_process_new(row['lon_acc'], row['ip_acc'], row['ip_dec']), axis=1) # for shake detector self.ego_df['cur_ego_path'] = self.ego_df.apply(self._cal_cur_ego_path, axis=1) self.ego_df['curvHor'] = self.ego_df['curvHor'].astype('float') self.ego_df['cur_diff'] = (self.ego_df['cur_ego_path'] - self.ego_df['curvHor']).abs() self.ego_df['R'] = self.ego_df['curvHor'].apply(lambda x: 10000 if x == 0 else 1 / x) self.ego_df['R_ego'] = self.ego_df['cur_ego_path'].apply(lambda x: 10000 if x == 0 else 1 / x) self.ego_df['R_diff'] = (self.ego_df['R_ego'] - self.ego_df['R']).abs() self.cur_ego_path_list = self.ego_df['cur_ego_path'].values.tolist() self.curvature_list = self.ego_df['curvHor'].values.tolist() def _peak_valley_determination(self, df): """ Determine the peak and valley of the vehicle based on its current angular velocity. Parameters: df: Dataframe containing the vehicle angular velocity. Returns: peak_valley: List of indices representing peaks and valleys. """ peaks, _ = scipy.signal.find_peaks(df['speedH'], height=0.01, distance=1, prominence=0.01) valleys, _ = scipy.signal.find_peaks(-df['speedH'], height=0.01, distance=1, prominence=0.01) peak_valley = sorted(list(peaks) + list(valleys)) return peak_valley def _peak_valley_judgment(self, p_last, p_curr, tw=10000, avg=0.02): """ Determine if the given peaks and valleys satisfy certain conditions. Parameters: p_last: Previous peak or valley data point. p_curr: Current peak or valley data point. tw: Threshold time difference between peaks and valleys. avg: Angular velocity gap threshold. Returns: Boolean indicating whether the conditions are satisfied. """ t_diff = p_curr[0] - p_last[0] v_diff = abs(p_curr[1] - p_last[1]) s = p_curr[1] * p_last[1] zigzag_flag = t_diff < tw and v_diff > avg and s < 0 if zigzag_flag and ([p_last[0], p_curr[0]] not in self.zigzag_time_list): self.zigzag_time_list.append([p_last[0], p_curr[0]]) return zigzag_flag @peak_valley_decorator def zigzag_count_func(self, p_curr, p_last, flag=True): """ Count the number of zigzag movements. Parameters: df: Input dataframe data. Returns: zigzag_count: Number of zigzag movements. """ if flag: self.zigzag_count += 1 else: self.zigzag_count += 0 @peak_valley_decorator def cal_zigzag_strength_strength(self, p_curr, p_last, flag=True): """ Calculate various strength statistics. Returns: Tuple containing maximum strength, minimum strength, average strength, and 99th percentile strength. """ if flag: v_diff = abs(p_curr[1] - p_last[1]) t_diff = p_curr[0] - p_last[0] if t_diff > 0: self.zigzag_stre_list.append(v_diff / t_diff) # 平均角加速度 else: self.zigzag_stre_list = [] def _shake_detector(self, Cr_diff=0.05, T_diff=0.39): """ ego车横向加速度ax; ego车轨迹横向曲率; ego车轨迹曲率变化率; ego车所在车lane曲率; ego车所在车lane曲率变化率; 转向灯（暂时存疑，可不用）Cr_diff = 0.1, T_diff = 0.04 求解曲率公式k(t) = (x'(t) * y''(t) - y'(t) * x''(t)) / ((x'(t))^2 + (y'(t))^2)^(3/2) """ time_list = [] frame_list = [] shake_time_list = [] df = self.ego_df.copy() df = df[df['cur_diff'] > Cr_diff] df['frame_ID_diff'] = df['simFrame'].diff() # 找出行车轨迹曲率与道路曲率之差大于阈值的数据段 filtered_df = df[df.frame_ID_diff > T_diff] # 此处是用大间隔区分多次晃动情景 。 row_numbers = filtered_df.index.tolist() cut_column = pd.cut(df.index, bins=row_numbers) grouped = df.groupby(cut_column) dfs = {} for name, group in grouped: dfs[name] = group.reset_index(drop=True) for name, df_group in dfs.items(): # 直道，未主动换道 df_group['curvHor'] = df_group['curvHor'].abs() df_group_straight = df_group[(df_group.lightMask == 0) & (df_group.curvHor < 0.001)] if not df_group_straight.empty: tmp_list = df_group_straight['simTime'].values # shake_time_list.append([tmp_list[0], tmp_list[-1]]) time_list.extend(df_group_straight['simTime'].values) frame_list.extend(df_group_straight['simFrame'].values) self.shake_count = self.shake_count + 1 # 打转向灯，道路为直道，此时晃动判断标准车辆曲率变化率为一个更大的阈值 df_group_change_lane = df_group[(df_group['lightMask'] != 0) & (df_group['curvHor'] < 0.001)] df_group_change_lane_data = df_group_change_lane[df_group_change_lane.cur_diff > Cr_diff + 0.2] if not df_group_change_lane_data.empty: tmp_list = df_group_change_lane_data['simTime'].values # shake_time_list.append([tmp_list[0], tmp_list[-1]]) time_list.extend(df_group_change_lane_data['simTime'].values) frame_list.extend(df_group_change_lane_data['simFrame'].values) self.shake_count = self.shake_count + 1 # 转弯，打转向灯 df_group_turn = df_group[(df_group['lightMask'] != 0) & (df_group['curvHor'].abs() > 0.001)] df_group_turn_data = df_group_turn[df_group_turn.cur_diff.abs() > Cr_diff + 0.1] if not df_group_turn_data.empty: tmp_list = df_group_turn_data['simTime'].values # shake_time_list.append([tmp_list[0], tmp_list[-1]]) time_list.extend(df_group_turn_data['simTime'].values) frame_list.extend(df_group_turn_data['simFrame'].values) self.shake_count = self.shake_count + 1 TIME_RANGE = 1 t_list = time_list f_list = frame_list group_time = [] group_frame = [] sub_group_time = [] sub_group_frame = [] for i in range(len(f_list)): if not sub_group_time or t_list[i] - t_list[i - 1] <= TIME_RANGE: sub_group_time.append(t_list[i]) sub_group_frame.append(f_list[i]) else: group_time.append(sub_group_time) group_frame.append(sub_group_frame) sub_group_time = [t_list[i]] sub_group_frame = [f_list[i]] # 输出图表值 shake_time = [[g[0], g[-1]] for g in group_time] shake_frame = [[g[0], g[-1]] for g in group_frame] self.shake_count = len(shake_time) if shake_time: time_df = pd.DataFrame(shake_time, columns=['start_time', 'end_time']) frame_df = pd.DataFrame(shake_frame, columns=['start_frame', 'end_frame']) discomfort_df = pd.concat([time_df, frame_df], axis=1) discomfort_df['type'] = 'shake' self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True) return time_list def _cadence_process(self, lon_acc_roc, ip_dec_roc): if abs(lon_acc_roc) >= abs(ip_dec_roc) or abs(lon_acc_roc) < 1: return np.nan # elif abs(lon_acc_roc) == 0: elif abs(lon_acc_roc) == 0: return 0 elif lon_acc_roc > 0 and lon_acc_roc < -ip_dec_roc: return 1 elif lon_acc_roc < 0 and lon_acc_roc > ip_dec_roc: return -1 def _cadence_process_new(self, lon_acc, ip_acc, ip_dec): if abs(lon_acc) < 1 or lon_acc > ip_acc or lon_acc < ip_dec: return np.nan # elif abs(lon_acc_roc) == 0: elif abs(lon_acc) == 0: return 0 elif lon_acc > 0 and lon_acc < ip_acc: return 1 elif lon_acc < 0 and lon_acc > ip_dec: return -1 else: return 0 def _cadence_detector(self): """ # 加速度突变：先加后减，先减后加，先加然后停，先减然后停 # 顿挫：2s内多次加速度变化率突变 # 求出每一个特征点，然后提取，然后将每一个特征点后面的2s做一个窗口，统计频率，避免无效运算 # 将特征点筛选出来 # 将特征点时间作为聚类标准，大于1s的pass，小于等于1s的聚类到一个分组 # 去掉小于3个特征点的分组 """ # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'cadence']].copy() data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'cadence']].copy() time_list = data['simTime'].values.tolist() data = data[data['cadence'] != np.nan] data['cadence_diff'] = data['cadence'].diff() data.dropna(subset='cadence_diff', inplace=True) data = data[data['cadence_diff'] != 0] t_list = data['simTime'].values.tolist() f_list = data['simFrame'].values.tolist() TIME_RANGE = 1 group_time = [] group_frame = [] sub_group_time = [] sub_group_frame = [] for i in range(len(f_list)): if not sub_group_time or t_list[i] - t_list[i - 1] <= TIME_RANGE: # 特征点相邻一秒内的，算作同一组顿挫 sub_group_time.append(t_list[i]) sub_group_frame.append(f_list[i]) else: group_time.append(sub_group_time) group_frame.append(sub_group_frame) sub_group_time = [t_list[i]] sub_group_frame = [f_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) >= 1] # 有一次特征点则算作一次顿挫 group_frame = [g for g in group_frame if len(g) >= 1] # 将顿挫组的起始时间为组重新统计时间 # 输出图表值 cadence_time = [[g[0], g[-1]] for g in group_time] cadence_frame = [[g[0], g[-1]] for g in group_frame] if cadence_time: time_df = pd.DataFrame(cadence_time, columns=['start_time', 'end_time']) frame_df = pd.DataFrame(cadence_frame, columns=['start_frame', 'end_frame']) discomfort_df = pd.concat([time_df, frame_df], axis=1) discomfort_df['type'] = 'cadence' self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True) # 将顿挫组的起始时间为组重新统计时间 cadence_time_list = [time for pair in cadence_time for time in time_list if pair[0] <= time <= pair[1]] # time_list = [element for sublist in group_time for element in sublist] # merged_list = [element for sublist in res_group for element in sublist] # res_df = data[data['simTime'].isin(merged_list)] stre_list = [] freq_list = [] for g in group_time: # calculate strength g_df = data[data['simTime'].isin(g)] strength = g_df['lon_acc'].abs().mean() stre_list.append(strength) # calculate frequency cnt = len(g) t_start = g_df['simTime'].iloc[0] t_end = g_df['simTime'].iloc[-1] t_delta = t_end - t_start frequency = cnt / t_delta freq_list.append(frequency) self.cadence_count = len(freq_list) cadence_stre = sum(stre_list) / len(stre_list) if stre_list else 0 return cadence_time_list def _slam_brake_detector(self): # 统计急刹全为1的分段的个数，记录分段开头的frame_ID # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'ip_dec_roc', 'slam_brake']].copy() data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'ip_dec', 'slam_brake']].copy() # data['slam_diff'] = data['slam_brake'].diff() # res_df = data[data['slam_diff'] == 1] res_df = data[data['slam_brake'] == 1] t_list = res_df['simTime'].values f_list = res_df['simFrame'].values.tolist() TIME_RANGE = 1 group_time = [] group_frame = [] sub_group_time = [] sub_group_frame = [] for i in range(len(f_list)): if not sub_group_time or f_list[i] - f_list[i - 1] <= TIME_RANGE: # 连续帧的算作同一组急刹 sub_group_time.append(t_list[i]) sub_group_frame.append(f_list[i]) else: group_time.append(sub_group_time) group_frame.append(sub_group_frame) sub_group_time = [t_list[i]] sub_group_frame = [f_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] # 输出图表值 slam_brake_time = [[g[0], g[-1]] for g in group_time] slam_brake_frame = [[g[0], g[-1]] for g in group_frame] if slam_brake_time: time_df = pd.DataFrame(slam_brake_time, columns=['start_time', 'end_time']) frame_df = pd.DataFrame(slam_brake_frame, columns=['start_frame', 'end_frame']) discomfort_df = pd.concat([time_df, frame_df], axis=1) discomfort_df['type'] = 'slam_brake' self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True) time_list = [element for sublist in group_time for element in sublist] self.slam_brake_count = len(group_time) # / self.mileage # * 1000000 return time_list def _slam_accel_detector(self): # 统计急刹全为1的分段的个数，记录分段开头的frame_ID # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'ip_acc_roc', 'slam_accel']].copy() data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'ip_acc', 'slam_accel']].copy() # data['slam_diff'] = data['slam_accel'].diff() # res_df = data.loc[data['slam_diff'] == 1] res_df = data.loc[data['slam_accel'] == 1] t_list = res_df['simTime'].values f_list = res_df['simFrame'].values.tolist() group_time = [] group_frame = [] sub_group_time = [] sub_group_frame = [] for i in range(len(f_list)): if not group_time or f_list[i] - f_list[i - 1] <= 1: # 连续帧的算作同一组急加速 sub_group_time.append(t_list[i]) sub_group_frame.append(f_list[i]) else: group_time.append(sub_group_time) group_frame.append(sub_group_frame) sub_group_time = [t_list[i]] sub_group_frame = [f_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] # 输出图表值 slam_accel_time = [[g[0], g[-1]] for g in group_time] slam_accel_frame = [[g[0], g[-1]] for g in group_frame] if slam_accel_time: time_df = pd.DataFrame(slam_accel_time, columns=['start_time', 'end_time']) frame_df = pd.DataFrame(slam_accel_frame, columns=['start_frame', 'end_frame']) discomfort_df = pd.concat([time_df, frame_df], axis=1) discomfort_df['type'] = 'slam_accel' self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True) time_list = [element for sublist in group_time for element in sublist] self.slam_accel_count = len(group_time) # / self.mileage # * 1000000 return time_list def comf_statistic(self): df = self.ego_df[['simTime', 'cur_diff', 'lon_acc', 'lon_acc_roc', 'accelH']].copy() self.zigzag_count_func() self.cal_zigzag_strength_strength() if self.zigzag_time_list: zigzag_df = pd.DataFrame(self.zigzag_time_list, columns=['start_time', 'end_time']) zigzag_df = get_frame_with_time(zigzag_df, self.ego_df) zigzag_df['type'] = 'zigzag' self.discomfort_df = pd.concat([self.discomfort_df, zigzag_df], ignore_index=True) # discomfort_df = pd.concat([time_df, frame_df], axis=1) # self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True) zigzag_t_list = [] # 只有[t_start, t_end]数对，要提取为完整time list t_list = df['simTime'].values.tolist() for t_start, t_end in self.zigzag_time_list: index_1 = t_list.index(t_start) index_2 = t_list.index(t_end) zigzag_t_list.extend(t_list[index_1:index_2 + 1]) zigzag_t_list = list(set(zigzag_t_list)) shake_t_list = self._shake_detector() cadence_t_list = self._cadence_detector() slam_brake_t_list = self._slam_brake_detector() slam_accel_t_list = self._slam_accel_detector() discomfort_time_list = zigzag_t_list + shake_t_list + cadence_t_list + slam_brake_t_list + slam_accel_t_list discomfort_time_list = sorted(discomfort_time_list) # 排序 discomfort_time_list = list(set(discomfort_time_list)) # 去重 # TIME_DIFF = self.time_list[3] - self.time_list[2] # TIME_DIFF = 0.4 FREQUENCY = 100 TIME_DIFF = 1 / FREQUENCY self.discomfort_duration = len(discomfort_time_list) * TIME_DIFF df['flag_zigzag'] = df['simTime'].apply(lambda x: 1 if x in zigzag_t_list else 0) df['flag_shake'] = df['simTime'].apply(lambda x: 1 if x in shake_t_list else 0) df['flag_cadence'] = df['simTime'].apply(lambda x: 1 if x in cadence_t_list else 0) df['flag_slam_brake'] = df['simTime'].apply(lambda x: 1 if x in slam_brake_t_list else 0) df['flag_slam_accel'] = df['simTime'].apply(lambda x: 1 if x in slam_accel_t_list else 0) self.calculated_value = { "weaving": self.zigzag_count, "shake": self.shake_count, "cadence": self.cadence_count, "slamBrake": self.slam_brake_count, "slamAccelerate": self.slam_accel_count } self.logger.info(f"舒适性计算完成，统计结果：{self.calculated_value}") return self.calculated_value def _nan_detect(self, num): if math.isnan(num): return 0 return num def zip_time_pairs(self, zip_list): zip_time_pairs = zip(self.time_list, zip_list) zip_vs_time = [[x, "" if math.isnan(y) else y] for x, y in zip_time_pairs] return zip_vs_time def report_statistic(self): comfort_result = self.comf_statistic() # comfort_config_path = self.config_path / "comfort_config.yaml" #"comfort_config.yaml" # "comfort_config.yaml" evaluator = Score(self.data_processed.comfort_config) result = evaluator.evaluate(comfort_result) print("\n[舒适性表现及得分情况]") return result if __name__ == '__main__': case_name = 'ICA' mode_label = 'PGVIL' data = data_process.DataPreprocessing(case_name, mode_label) comfort_instance = Comfort(data) # 调用实例方法 report_statistic，它不接受除 self 之外的参数 try: comfort_result = comfort_instance.report_statistic() result = {'comfort': comfort_result} except Exception as e: print(f"An error occurred in Comfort.report_statistic: {e}")