lantu_vehstate/cicv_ica_longitudinal_control07_rise_time_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 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
Max_Time = 1000
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.df = pd.DataFrame()
        self.ego_df = pd.DataFrame()
        self.df_ica = pd.DataFrame()

        # self.stable_start_time_THW = None
        self.stable_average_THW = None
        self.rise_time_THW = None

        self.result = {
            "name": "跟车上升时间",
            "value": [],
            # "weight": [],
            "tableData": {
                "avg": "",  # 平均值，或指标值
                "max": "",
                "min": ""
            },
            "reportData": {
                "name": "跟车上升时间（s）",
                # "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['ICA']['ICA_follow_time']
        self.df_ica = get_status_active_data(active_time_ranges, self.ego_df)

        # self.df_ica = self.ego_df[self.ego_df['ICA_status'] == "LLC_Follow_Vehicle"].copy()  # 数字3对应ICA的Active
        # self.df_ica = self.df[self.df['ACC_status'] == "Shut_off"].copy()  # 数字3对应ICA的Active
        # self.df_ica = self.df[self.df['ACC_status'] == "Active"].copy()  # 数字3对应ICA的Active

        if self.df_ica.empty:
            self.result['statusFlag']['function_ICA'] = False
        else:
            self.result['statusFlag']['function_ICA'] = True

    def _get_first_change_index_THW(self):
        """
        获取DataFrame中'set_headway_time'列首次发生变化的索引值。

        Args:
            无参数。

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

        """
        change_indices = self.df_ica[self.df_ica['set_headway_time'] != self.df_ica['set_headway_time'].shift()].index
        if not change_indices.empty:
            first_change_index = change_indices.min()
        else:
            first_change_index = None
        return first_change_index

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

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

        Returns:
            None

        """
        ego_x = self.df_ica[self.df_ica['playerId'] == 1]['posX'].reset_index(drop=True)
        ego_y = self.df_ica[self.df_ica['playerId'] == 1]['posY'].reset_index(drop=True)
        obj_x = self.df_ica[self.df_ica['playerId'] == 2]['posX'].reset_index(drop=True)
        obj_y = self.df_ica[self.df_ica['playerId'] == 2]['posY'].reset_index(drop=True)

        ego_speedx = self.df_ica[self.df_ica['playerId'] == 1]['speedX'].reset_index(drop=True)
        ego_speedy = self.df_ica[self.df_ica['playerId'] == 1]['speedY'].reset_index(drop=True)
        obj_speedx = self.df_ica[self.df_ica['playerId'] == 2]['speedX'].reset_index(drop=True)
        obj_speedy = self.df_ica[self.df_ica['playerId'] == 2]['speedY'].reset_index(drop=True)

        dx = obj_x - ego_x
        dy = obj_y - ego_y
        vx = obj_speedx - ego_speedx
        vy = obj_speedy - ego_speedy
        dist = np.sqrt(dx ** 2 + dy ** 2)
        ego_v_projection_in_dist = _cal_v_ego_projection(dx, dy, ego_speedx, ego_speedy)
        thw1 = _cal_THW(dist, ego_v_projection_in_dist)
        thw = thw1.tolist()
        THW = []
        for item in thw:
            THW.append(item)
            THW.append(item)
        self.df_ica['THW'] = THW

        THW = self.df_ica['THW'].values
        deviation = set_value * (percent_deviation / 100)
        stable_start = 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_start_time_THW = self.df_ica['simTime'].iloc[stable_start]
        self.stable_average_THW = stable_average_THW

    def _find_closest_time_stamp_THW(self, df, target_THW, start_index):
        """
        在DataFrame中找到与目标THW值最接近的时间戳。

        Args:
            df (pandas.DataFrame): 包含'THW'和'simTime'列的DataFrame，其中'THW'表示目标变量，'simTime'表示时间戳。
            target_THW (float): 目标THW值。
            start_index (int): 开始搜索的索引位置（不包含）。

        Returns:
            pandas.Timestamp: 与目标THW值最接近的时间戳。

        """
        subset = df.loc[start_index + 1:]
        THW_diff = np.abs(subset['THW'] - target_THW)
        closest_index = THW_diff.idxmin()
        closest_timestamp = subset.loc[closest_index, 'simTime']
        return closest_timestamp

    def data_analyze(self):
        """
        计算巡航时从初THW到达稳定THW的90%的所需时间（rise time）。

        Args:
            无。

        Returns:
            无返回值，但会设置实例属性self.rise_time_THW为计算得到的rise time。

        """
        if self.df_ica.empty:
            self.result['value'] = [0.0]
            print(f"Rise time: 0")
        else:
            first_change_index = self._get_first_change_index_THW()

            set_headway_time = self.df_ica.loc[first_change_index, 'set_headway_time']
            self._find_stable_THW(window_size=4, percent_deviation=5, set_value=set_headway_time)

            initial_THW = self.df_ica.loc[first_change_index, 'THW']
            if self.stable_average_THW:
                target_THW_90 = initial_THW + (self.stable_average_THW - initial_THW) * 0.9
                target_THW_10 = initial_THW + (self.stable_average_THW - initial_THW) * 0.1

                timestamp_at_10 = self._find_closest_time_stamp_THW(self.df_ica, target_THW_10, first_change_index)
                timestamp_at_90 = self._find_closest_time_stamp_THW(self.df_ica, target_THW_90, first_change_index)

                print(f"Closest speed at 10% range from set speed: {timestamp_at_10}")
                print(f"Closest speed at 90% range from set speed: {timestamp_at_90}")

                self.rise_time_THW = timestamp_at_90 - timestamp_at_10
            else:
                self.rise_time_THW = Max_Time
            self.result['value'] = [round(self.rise_time_THW, 3)]
            print(f"Rise time: {self.rise_time_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.df_ica.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:[rise_time_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