zhaoyuan_pre_acceptance/modules/metric/comfort.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import math
import pandas as pd
import numpy as np
import scipy.signal
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.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, 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.data_processed = data_processed
        self.logger = LogManager().get_logger()

        self.data = data_processed.ego_data.copy()
        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 = self.data['simTime'].values.tolist()
        self.frame_list = self.data['simFrame'].values.tolist()

        # 移除未使用的字典
        self.zigzag_count = 0
        self.shake_count = 0
        self.cadence_count = 0
        self.slam_brake_count = 0
        self.slam_accel_count = 0

        self.zigzag_time_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)

    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:
            return 1e5  # 异常时也返回极大值（如除零、缺失值等）

    def _comf_param_cal(self):
        """计算舒适性相关参数"""
        # 加减速阈值计算
        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)

        # 晃动检测相关参数
        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):
        """
        确定车辆角速度的峰值和谷值
        """
        # 调整参数以减少噪音干扰
        peaks, _ = scipy.signal.find_peaks(df['speedH'], height=0.03, distance=3, prominence=0.03, width=1)
        valleys, _ = scipy.signal.find_peaks(-df['speedH'], height=0.03, distance=3, prominence=0.03, width=1)
        peak_valley = sorted(list(peaks) + list(valleys))
        return peak_valley

    def _peak_valley_judgment(self, p_last, p_curr, tw=100, avg=0.06):
        """
        判断给定的峰值和谷值是否满足特定条件
        """
        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):
        """计算曲折行驶次数"""
        if flag:
            self.zigzag_count += 1

    @peak_valley_decorator
    def cal_zigzag_strength_strength(self, p_curr, p_last, flag=True):
        """计算曲折行驶强度"""
        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):
        """检测晃动事件"""
        time_list = []
        frame_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:
                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:
                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:
                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_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) == 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):
        """检测顿挫事件"""
        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'cadence', 'v']].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 self.ego_df['simTime'].values if pair[0] <= time <= pair[1]]

        self.cadence_count = len(cadence_time)

        return cadence_time_list

    def _slam_brake_detector(self):
        """检测急刹车事件"""
        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'ip_dec', 'slam_brake', 'v']].copy()
        
        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)
        return time_list

    def _slam_accel_detector(self):
        """检测急加速事件"""
        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'ip_acc', 'slam_accel', 'v']].copy()
        
        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)
        return time_list
    
    def comf_statistic(self):
        """统计舒适性指标"""
        df = self.ego_df[['simTime', 'simFrame', 'cur_diff', 'lon_acc', 'lon_acc_roc', 'accelH', 'speedH', 'lat_acc', 'v']].copy()

        self.zigzag_count_func()
        self.cal_zigzag_strength_strength()
        if self.zigzag_time_list:
            # 保存 Weaving (zigzag) 事件摘要
            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)

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

        # 统计结果
        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 report_statistic(self):
        """生成舒适性评估报告"""
        comfort_result = self.comf_statistic()
        evaluator = Score(self.data_processed.comfort_config)
        result = evaluator.evaluate(comfort_result) 
        print("\n[舒适性表现及得分情况]")
        return result