#!/usr/bin/env python # -*- coding: utf-8 -*- ################################################################## # # Copyright (c) 2025 CICV, Inc. All Rights Reserved # ################################################################## """ @Authors: zhanghaiwen(zhanghaiwen@china-icv.cn) @Data: 2025/01/5 @Last Modified: 2025/01/5 @Summary: Function Metrics Calculation """ import sys from pathlib import Path # 添加项目根目录到系统路径 root_path = Path(__file__).resolve().parent.parent sys.path.append(str(root_path)) print(root_path) from modules.lib.score import Score from modules.lib.log_manager import LogManager import numpy as np from typing import Dict, Tuple, Optional, Callable, Any import pandas as pd import yaml from modules.lib.chart_generator import generate_function_chart_data from shapely.geometry import Point, Polygon from modules.lib.common import get_interpolation import pandas as pd import yaml import math # ---------------------- # 基础工具函数 (Pure functions) # ---------------------- scenario_sign_dict = { "LeftTurnAssist": 206, "HazardousLocationW": 207, "RedLightViolationW": 208, "AbnormalVehicleW": 209, "NsightVulnerableRoadUserCollisionW": 210, "LitterW": 211, "ForwardCollision": 212, "VisibilityW": 213, "EmergencyBrakeW": 214, "IntersectionCollisionW": 215, "BlindSpotW": 216, "DoNotPassW": 217, "ControlLossW": 218, "FrontTrafficJamW": 219, "EmergencyVehicleW": 220, "CooperativeVehicleMerge": 221, "CooperativeLaneChange": 223, "VulnerableRoadUserCollisionW": 224, "CooperativeIntersectionPassing": 225, "RampMerge": 226, "DrivingLaneRecommendation": 227, "TrafficJamW": 228, "DynamicSpeedLimitingInformation": 229, "EmergencyVehiclesPriority": 230, "RemoteSupervision": 231, "SignalLightReminder": 232, "OtherVehicleRedLightViolationW": 233, "GreenLightOptimalSpeedAdvisory": 234, } def _is_pedestrian_in_crosswalk(polygon, test_point) -> bool: polygon = Polygon(polygon) point = Point(test_point) return polygon.contains(point) def _is_segment_by_interval(time_list, expected_step) -> list: """ 根据时间戳之间的间隔进行分段。 参数: time_list (list): 时间戳列表。 expected_step (float): 预期的固定步长。 返回: list: 分段后的时间戳列表，每个元素是一个子列表。 """ if not time_list: return [] segments = [] current_segment = [time_list[0]] for i in range(1, len(time_list)): actual_step = time_list[i] - time_list[i - 1] if actual_step != expected_step: # 如果间隔不符合预期，则开始一个新的段 segments.append(current_segment) current_segment = [time_list[i]] else: # 否则，将当前时间戳添加到当前段中 current_segment.append(time_list[i]) # 添加最后一个段 if current_segment: segments.append(current_segment) return segments # 寻找二级指标的名称 def find_nested_name(data): """ 查找字典中嵌套的name结构。 :param data: 要搜索的字典 :return: 找到的第一个嵌套name结构的值，如果没有找到则返回None """ if isinstance(data, dict): for key, value in data.items(): if isinstance(value, dict) and 'name' in value: return value['name'] # 递归查找嵌套字典 result = find_nested_name(value) if result is not None: return result elif isinstance(data, list): for item in data: result = find_nested_name(item) if result is not None: return result return None def calculate_distance_PGVIL(ego_pos: np.ndarray, obj_pos: np.ndarray) -> np.ndarray: """向量化距离计算""" return np.linalg.norm(ego_pos - obj_pos, axis=1) def calculate_relative_speed_PGVIL( ego_speed: np.ndarray, obj_speed: np.ndarray ) -> np.ndarray: """向量化相对速度计算""" return np.linalg.norm(ego_speed - obj_speed, axis=1) def calculate_distance(ego_df: pd.DataFrame, correctwarning: int) -> np.ndarray: """向量化距离计算""" dist = ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['ifwarning'].notna())]['relative_dist'] return dist def calculate_relative_speed(ego_df: pd.DataFrame, correctwarning: int) -> np.ndarray: """向量化相对速度计算""" return ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['ifwarning'].notna())]['composite_v'] def extract_ego_obj(data: pd.DataFrame) -> Tuple[pd.Series, pd.DataFrame]: """数据提取函数""" ego = data[data["playerId"] == 1].iloc[0] obj = data[data["playerId"] != 1] return ego, obj def get_first_warning(data_processed) -> Optional[pd.DataFrame]: """带缓存的预警数据获取""" ego_df = data_processed.ego_data obj_df = data_processed.object_df scenario_name = find_nested_name(data_processed.function_config["function"]) correctwarning = scenario_sign_dict.get(scenario_name) if correctwarning is None: print("无法获取正确的预警信号标志位！") return None warning_rows = ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['ifwarning'].notna())] warning_times = warning_rows['simTime'] if warning_times.empty: print("没有找到预警数据！") return None first_time = warning_times.iloc[0] return obj_df[obj_df['simTime'] == first_time] def getAxis(heading): AxisL = [0, 0] AxisW = [0, 0] AxisL[0] = math.cos(heading) AxisL[1] = math.sin(heading) AxisW[0] = -math.sin(heading) AxisW[1] = math.cos(heading) return AxisL, AxisW def getDotMultiply(firstAxis, secondAxis): dotMultiply = abs(firstAxis[0] * secondAxis[0] + firstAxis[1] * secondAxis[1]) return dotMultiply def getProjectionRadius(AxisL, AxisW, baseAxis, halfLength, halfWidth): projAxisL = getDotMultiply(AxisL, baseAxis) projAxisW = getDotMultiply(AxisW, baseAxis) projectionRadius = halfLength * projAxisL + halfWidth * projAxisW return projectionRadius def isCollision( firstAxisL, firstAxisW, firstHalfLength, firstHalfWidth, secondAxisL, secondAxisW, secondHalfLength, secondHalfWidth, disVector, ): isCollision = True axes = [firstAxisL, firstAxisW, secondAxisL, secondAxisW] addProjectionRadius = 0 projectionDisVecotr = 0 for axis in axes: addProjectionRadius = getProjectionRadius( firstAxisL, firstAxisW, axis, firstHalfLength, firstHalfWidth ) + getProjectionRadius( secondAxisL, secondAxisW, axis, secondHalfLength, secondHalfWidth ) projectionDisVecotr = getDotMultiply(disVector, axis) if projectionDisVecotr > addProjectionRadius: isCollision = False break return isCollision def funcIsCollision( firstDimX, firstDimY, firstOffX, firstOffY, firstX, firstY, firstHeading, secondDimX, secondDimY, secondOffX, secondOffY, secondX, secondY, secondHeading, ): firstAxisL = getAxis(firstHeading)[0] firstAxisW = getAxis(firstHeading)[1] secondAxisL = getAxis(secondHeading)[0] secondAxisW = getAxis(secondHeading)[1] firstHalfLength = 0.5 * firstDimX firstHalfWidth = 0.5 * firstDimY secondHalfLength = 0.5 * secondDimX secondHalfWidth = 0.5 * secondDimY firstCenter = [0, 0] secondCenter = [0, 0] disVector = [0, 0] firstCenter[0] = firstX + firstOffX * math.cos(firstHeading) firstCenter[1] = firstY + firstOffX * math.sin(firstHeading) secondCenter[0] = secondX + secondOffX * math.cos(secondHeading) secondCenter[1] = secondY + secondOffX * math.sin(secondHeading) disVector[0] = secondCenter[0] - firstCenter[0] disVector[1] = secondCenter[1] - firstCenter[1] varIsCollision = isCollision( firstAxisL, firstAxisW, firstHalfLength, firstHalfWidth, secondAxisL, secondAxisW, secondHalfLength, secondHalfWidth, disVector, ) return varIsCollision # ---------------------- # 核心计算功能函数 # ---------------------- def latestWarningDistance_LST(data, plot_path) -> dict: """预警距离计算流水线""" scenario_name = find_nested_name(data.function_config["function"]) value = data.function_config["function"][scenario_name]["latestWarningDistance_LST"]["max"] correctwarning = scenario_sign_dict[scenario_name] ego_df = data.ego_data warning_dist = calculate_distance(ego_df, correctwarning) warning_speed = calculate_relative_speed(ego_df, correctwarning) # 将计算结果保存到data对象中，供图表生成使用 data.warning_dist = warning_dist data.warning_speed = warning_speed data.correctwarning = correctwarning if len(warning_dist.tolist()) == 0: return {"latestWarningDistance_LST": 0.0} # 生成图表数据 generate_function_chart_data(data, 'latestWarningDistance_LST', plot_path) return {"latestWarningDistance_LST": warning_dist.tolist()[-1] if len(warning_dist) > 0 else value} def earliestWarningDistance_LST(data, plot_path) -> dict: """预警距离计算流水线""" scenario_name = find_nested_name(data.function_config["function"]) value = data.function_config["function"][scenario_name]["earliestWarningDistance_LST"]["max"] correctwarning = scenario_sign_dict[scenario_name] ego_df = data.ego_data warning_dist = calculate_distance(ego_df, correctwarning) warning_speed = calculate_relative_speed(ego_df, correctwarning) # 将计算结果保存到data对象中，供图表生成使用 data.warning_dist = warning_dist data.warning_speed = warning_speed data.correctwarning = correctwarning if len(warning_dist.tolist()) == 0: return {"earliestWarningDistance_LST": 0.0} # 生成图表数据 generate_function_chart_data(data, 'earliestWarningDistance_LST', plot_path) return {"earliestWarningDistance_LST": float(warning_dist.tolist()[0]) if len(warning_dist) > 0 else value} def latestWarningDistance_TTC_LST(data, plot_path) -> dict: """TTC计算流水线""" scenario_name = find_nested_name(data.function_config["function"]) value = data.function_config["function"][scenario_name]["latestWarningDistance_TTC_LST"]["max"] correctwarning = scenario_sign_dict[scenario_name] ego_df = data.ego_data warning_dist = calculate_distance(ego_df, correctwarning) if len(warning_dist.tolist()) == 0: return {"latestWarningDistance_TTC_LST": 0.0} # 将correctwarning保存到data对象中，供图表生成使用 data.correctwarning = correctwarning warning_speed = calculate_relative_speed(ego_df, correctwarning) with np.errstate(divide='ignore', invalid='ignore'): ttc = np.where(warning_speed != 0, warning_dist / warning_speed, np.inf) # 处理无效的TTC值 for i in range(len(ttc)): ttc[i] = float(value) if (not ttc[i] or ttc[i] < 0) else ttc[i] data.warning_dist = warning_dist data.warning_speed = warning_speed data.ttc = ttc # 生成图表数据 # from modules.lib.chart_generator import generate_function_chart_data generate_function_chart_data(data, 'latestWarningDistance_TTC_LST', plot_path) return {"latestWarningDistance_TTC_LST": float(ttc[-1]) if len(ttc) > 0 else value} def earliestWarningDistance_TTC_LST(data, plot_path) -> dict: """TTC计算流水线""" scenario_name = find_nested_name(data.function_config["function"]) value = data.function_config["function"][scenario_name]["earliestWarningDistance_TTC_LST"]["max"] correctwarning = scenario_sign_dict[scenario_name] ego_df = data.ego_data warning_dist = calculate_distance(ego_df, correctwarning) if len(warning_dist.tolist()) == 0: return {"earliestWarningDistance_TTC_LST": 0.0} # 将correctwarning保存到data对象中，供图表生成使用 data.correctwarning = correctwarning warning_speed = calculate_relative_speed(ego_df, correctwarning) with np.errstate(divide='ignore', invalid='ignore'): ttc = np.where(warning_speed != 0, warning_dist / warning_speed, np.inf) # 处理无效的TTC值 for i in range(len(ttc)): ttc[i] = float(value) if (not ttc[i] or ttc[i] < 0) else ttc[i] # 将计算结果保存到data对象中，供图表生成使用 data.warning_dist = warning_dist data.warning_speed = warning_speed data.ttc = ttc data.correctwarning = correctwarning # 生成图表数据 generate_function_chart_data(data, 'earliestWarningDistance_TTC_LST', plot_path) return {"earliestWarningDistance_TTC_LST": float(ttc[0]) if len(ttc) > 0 else value} def warningDelayTime_LST(data, plot_path): scenario_name = find_nested_name(data.function_config["function"]) correctwarning = scenario_sign_dict[scenario_name] # 将correctwarning保存到data对象中，供图表生成使用 data.correctwarning = correctwarning ego_df = data.ego_data HMI_warning_rows = ego_df[(ego_df['ifwarning'] == correctwarning)]['simTime'].tolist() simTime_HMI = HMI_warning_rows[0] if len(HMI_warning_rows) > 0 else None rosbag_warning_rows = ego_df[(ego_df['event_Type'].notna()) & ((ego_df['event_Type'] != np.nan))][ 'simTime'].tolist() simTime_rosbag = rosbag_warning_rows[0] if len(rosbag_warning_rows) > 0 else None if (simTime_HMI is None) or (simTime_rosbag is None): print("预警出错！") delay_time = 100.0 else: delay_time = abs(simTime_HMI - simTime_rosbag) return {"warningDelayTime_LST": delay_time} def warningDelayTimeofReachDecel_LST(data, plot_path): scenario_name = find_nested_name(data.function_config["function"]) correctwarning = scenario_sign_dict[scenario_name] # 将correctwarning保存到data对象中，供图表生成使用 data.correctwarning = correctwarning ego_df = data.ego_data obj_df = data.obj_data[2] # 加速度在速度方向上的投影 speed_mag = np.sqrt(obj_df["speedX"] ** 2 + obj_df["speedY"] ** 2) + 1e-6 obj_df["ux"] = obj_df["speedX"] / speed_mag obj_df["uy"] = obj_df["speedY"] / speed_mag obj_df["merged_accel"] = obj_df["accelX"] * obj_df["ux"] + obj_df["accelY"] * obj_df["uy"] obj_speed_simtime = obj_df[obj_df['merged_accel'] <= -4]['simTime'].tolist() # 单位m/s^2 warning_simTime = ego_df[ego_df['ifwarning'] == correctwarning]['simTime'].tolist() if (len(warning_simTime) == 0) and (len(obj_speed_simtime) == 0): return {"warningDelayTimeofReachDecel_LST": 0} elif (len(warning_simTime) == 0) and (len(obj_speed_simtime) > 0): return {"warningDelayTimeofReachDecel_LST": obj_speed_simtime[0]} elif (len(warning_simTime) > 0) and (len(obj_speed_simtime) == 0): return {"warningDelayTimeofReachDecel_LST": -1} else: return {"warningDelayTimeofReachDecel_LST": warning_simTime[0] - obj_speed_simtime[0]} def rightWarningSignal_LST(data, plot_path): scenario_name = find_nested_name(data.function_config["function"]) correctwarning = scenario_sign_dict[scenario_name] # 将correctwarning保存到data对象中，供图表生成使用 data.correctwarning = correctwarning ego_df = data.ego_data if ego_df['ifwarning'].empty: print("无法获取正确预警信号标志位！") return warning_rows = ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['ifwarning'].notna())] if warning_rows.empty: return {"rightWarningSignal_LST": -1} else: return {"rightWarningSignal_LST": 1} def noWarning_LST(data, plot_path): scenario_name = find_nested_name(data.function_config["function"]) correctwarning = scenario_sign_dict[scenario_name] ego_df = data.ego_data if ego_df['ifwarning'].empty: print("无法获取正确预警信号标志位！") return warning_rows = (ego_df['ifwarning'].dropna() == -1).all() if warning_rows: return {"noWarning_LST": 1} else: return {"noWarning_LST": -1} # def ifCrossingRedLight_LST(data, plot_path): # scenario_name = find_nested_name(data.function_config["function"]) # correctwarning = scenario_sign_dict[scenario_name] # # 将correctwarning保存到data对象中，供图表生成使用 # data.correctwarning = correctwarning # ego_df = data.ego_data # redlight_simtime = ego_df[ # (ego_df['ifwarning'] == correctwarning) & (ego_df['stateMask'] == 1) & (ego_df['relative_dist'] <= 0.5) & ( # ego_df['v'] != 0)]['simTime'] # if redlight_simtime.empty: # return {"ifCrossingRedLight_LST": -1} # else: # return {"ifCrossingRedLight_LST": 1} def ifStopgreenWaveSpeedGuidance_LST(data, plot_path): scenario_name = find_nested_name(data.function_config["function"]) correctwarning = scenario_sign_dict[scenario_name] # 将correctwarning保存到data对象中，供图表生成使用 data.correctwarning = correctwarning ego_df = data.ego_data greenlight_simtime = \ ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['stateMask'] == 0) & (ego_df['v'] <= 0.1)]['simTime'] if greenlight_simtime.empty: return {"ifStopgreenWaveSpeedGuidance_LST": -1} else: return {"ifStopgreenWaveSpeedGuidance_LST": 1} # ------ 单车智能指标 ------ def limitSpeed_LST(data, plot_path): ego_df = data.ego_data scenario_name = find_nested_name(data.function_config["function"]) limit_speed = data.function_config["function"][scenario_name]["limitSpeed_LST"]["max"] speed_limit = ego_df[abs(ego_df['x_relative_dist']) <= 0.2]['v'].tolist() if len(speed_limit) == 0: return {"limitSpeed_LST": -1} max_speed = max(speed_limit)*3.6 data.speedLimit = limit_speed generate_function_chart_data(data, 'limitspeed_LST', plot_path) return {"limitSpeed_LST": max_speed} def limitSpeedPastLimitSign_LST(data, plot_path): ego_df = data.ego_data scenario_name = find_nested_name(data.function_config["function"]) limit_speed = data.function_config["function"][scenario_name]["limitSpeed_LST"]["max"] car_length = data.function_config["vehicle"]['CAR_LENGTH'] ego_speed = ego_df[ego_df['x_relative_dist'] <= -100 - car_length]['v'].tolist() ego_time = ego_df[ego_df['x_relative_dist'] <= -100 - car_length]['simTime'].tolist() data.speedLimit = limit_speed data.speedPastLimitSign_LST = ego_time[0] if len(ego_time) > 0 else None generate_function_chart_data(data, 'limitSpeedPastLimitSign_LST', plot_path) if len(ego_speed) == 0: return {"speedPastLimitSign_LST": -1} return {"speedPastLimitSign_LST": ego_speed[0]} def leastDistance_LST(data, plot_path): ego_df = data.ego_data dist_row = ego_df[ego_df['v'] <= 0.1]['relative_dist'].tolist() if len(dist_row) == 0: return {"leastDistance_LST": -1} else: min_dist = min(dist_row) return {"leastDistance_LST": min_dist} def noCrossRedLight_LST(data, plot_path): ego_df = data.ego_data cross_redlight_dist = ego_df[(ego_df['stateMask'] == 3) & (ego_df['filtered_trafficlight_id'] is not None) & abs(ego_df['v'] > 0.1)]['x_relative_dist'] has_positive = any(x > 0 for x in cross_redlight_dist) has_negative = any(x < 0 for x in cross_redlight_dist) has_both = has_positive and has_negative if has_both: return {'noCrossRedLight_LST': -1} else: return {'noCrossRedLight_LST': 1} def launchTimeinStopLine_LST(data, plot_path): ego_df = data.ego_data simtime_row = ego_df[ego_df['v'] <= 0.1]['simTime'].tolist() if len(simtime_row) == 0: return {"launchTimeinStopLine_LST": -1} else: delta_t = simtime_row[-1] - simtime_row[0] return {"launchTimeinStopLine_LST": delta_t} def launchTimewhenFollowingCar_LST(data, plot_path): ego_df = data.ego_data t_interval = ego_df['simTime'].tolist()[1] - ego_df['simTime'].tolist()[0] simtime_row = ego_df[ego_df['v'] <= 0.1]['simTime'].tolist() if len(simtime_row) == 0: return {"launchTimewhenFollowingCar_LST": 0} else: time_interval = _is_segment_by_interval(simtime_row, t_interval) delta_t = [] for t in time_interval: delta_t.append(t[-1] - t[0]) return {"launchTimewhenFollowingCar_LST": max(delta_t)} def noStop_LST(data, plot_path): ego_df_ini = data.ego_data min_time = ego_df_ini['simTime'].min() + 0 max_time = ego_df_ini['simTime'].max() - 0 ego_df = ego_df_ini[(ego_df_ini['simTime'] >= min_time) & (ego_df_ini['simTime'] <= max_time)] speed = ego_df['v'].tolist() if (any(speed) <= 0.5): return {"noStop_LST": -1} else: return {"noStop_LST": 1} def launchTimeinTrafficLight_LST(data, plot_path): ''' 待修改： 红灯的状态值：1 绿灯的状态值：0 ''' ego_df = data.ego_data simtime_of_redlight = ego_df[(ego_df['stateMask'] == 3) & (ego_df['filtered_trafficlight_id'] is not None)]['simTime'] simtime_of_stop = ego_df[ego_df['v'] <= 0.1]['simTime'] if len(simtime_of_stop) or len(simtime_of_redlight): return {"launchTimeinTrafficLight_LST": -1} simtime_of_launch = simtime_of_redlight[simtime_of_redlight.isin(simtime_of_stop)].tolist() if len(simtime_of_launch) == 0: return {"launchTimeinTrafficLight_LST": -1} return {"launchTimeinTrafficLight_LST": simtime_of_launch[-1] - simtime_of_launch[0]} def crossJunctionToTargetLane_LST(data, plot_path): ego_df = data.ego_data lane_in_leftturn = set(ego_df['lane_id'].tolist()) scenario_name = find_nested_name(data.function_config["function"]) if (30225902 in lane_in_leftturn) or (30226005 in lane_in_leftturn): return {"crossJunctionToTargetLane_LST": 1} else: return {"crossJunctionToTargetLane_LST": -1} def keepInLane_LST(data, plot_path): # ego_df = data.ego_data[data.ego_data['road_type'] == 15] ego_df = data.ego_data[data.ego_data['lane_id'].isin([30225904, 30225978])].copy() notkeepinlane = ego_df[ego_df['laneOffset'] > ego_df['lane_width'] / 2] if len(notkeepinlane) == 0: return {"keepInLane_LST": -1} return {"keepInLane_LST": 1} def leastLateralDistance_LST(data, plot_path): ego_df = data.ego_data lane_width = ego_df[abs(ego_df['x_relative_dist']) <= 0.5]['lane_width'].tolist()[0] if len(lane_width) == 0: return {"leastLateralDistance_LST": -1} else: y_relative_dist = ego_df[abs(ego_df['x_relative_dist']) <= 0.05]['y_relative_dist'] if (abs(y_relative_dist) <= lane_width / 2).all(): return {"leastLateralDistance_LST": 1} else: return {"leastLateralDistance_LST": -1} def waitTimeAtCrosswalkwithPedestrian_LST(data, plot_path): ego_df = data.ego_data object_df = data.obj_data data['in_crosswalk'] = [] position_data = data.drop_duplicates(subset=['cross_id', 'cross_coords'], inplace=True) for cross_id in position_data['cross_id'].tolist(): for posX, posY in object_df['posX', 'posY']: pedestrian_pos = (posX, posY) plogan_pos = position_data[position_data['cross_id'] == cross_id]['cross_coords'].tolist()[0] if _is_pedestrian_in_crosswalk(plogan_pos, pedestrian_pos): data[data['playerId'] == 2]['in_crosswalk'] = 1 else: data[data['playerId'] == 2]['in_crosswalk'] = 0 car_stop_time = ego_df[ego_df['v'] <= 0.1]['simTime'] pedestrian_in_crosswalk_time = data[(data['in_crosswalk'] == 1)]['simTime'] car_wait_pedestrian = car_stop_time.isin(pedestrian_in_crosswalk_time).tolist() return {'waitTimeAtCrosswalkwithPedestrian_LST': car_wait_pedestrian[-1] - car_wait_pedestrian[0] if len( car_wait_pedestrian) > 0 else 0} def launchTimewhenPedestrianLeave_LST(data, plot_path): ego_df = data.ego_data car_stop_time = ego_df[ego_df['v'] <= 0.1]["simTime"] if len(car_stop_time) == 0: return {"launchTimewhenPedestrianLeave_LST": 0} else: lane_width = ego_df[ego_df['v'] <= 0.1]['lane_width'].tolist()[0] car_to_pedestrain = ego_df[abs(ego_df['y_relative_dist']) <= lane_width / 2]["simTime"] legal_stop_time = car_stop_time.isin(car_to_pedestrain).tolist() return {"launchTimewhenPedestrianLeave_LST": legal_stop_time[-1] - legal_stop_time[0]} def noCollision_LST(data, plot_path): ego_df = data.ego_data is_dist_min = False is_sign_change = False for i in range(1, len(ego_df) - 1): relative_dist_decreasing = ego_df['relative_dist'][i-1] > ego_df['relative_dist'][i] relative_dist_increasing = ego_df['relative_dist'][i+1] > ego_df['relative_dist'][i] is_dist_min = relative_dist_decreasing and relative_dist_increasing and (ego_df['relative_dist'][i] < 0.5) x_relative = (ego_df['x_relative_dist'][i-1]*ego_df['x_relative_dist'][i]) < 0 y_relative = (ego_df['y_relative_dist'][i-1]*ego_df['y_relative_dist'][i]) < 0 is_sign_change = x_relative or y_relative if is_dist_min and is_sign_change: return {"noCollision_LST": -1} return {"noCollision_LST": 1} def noReverse_LST(data, plot_path): ego_df = data.ego_data if (ego_df["lon_v_vehicle"] * ego_df["posH"]).any() < 0: return {"noReverse_LST": -1} else: return {"noReverse_LST": 1} def turnAround_LST(data, plot_path): ego_df = data.ego_data if (ego_df["lon_v_vehicle"].tolist()[0] * ego_df["lon_v_vehicle"].tolist()[-1] < 0) and ( ego_df["lon_v_vehicle"] * ego_df["posH"].all() > 0): return {"turnAround_LST": 1} else: return {"turnAround_LST": -1} def laneOffset_LST(data, plot_path): car_width = data.vehicle_config['CAR_WIDTH'] ego_df_ini = data.ego_data min_time = ego_df_ini['simTime'].min() + 0 max_time = ego_df_ini['simTime'].max() - 0 ego_df = ego_df_ini[(ego_df_ini['simTime'] >= min_time) & (ego_df_ini['simTime'] <= max_time)] laneoffset = abs(ego_df['y_relative_dist']) - car_width / 2 return {"laneOffset_LST": max(laneoffset)} def maxLongitudeDist_LST(data, plot_path): ego_df = data.ego_data longitude_dist = abs(ego_df[ego_df['v'] <= 0.2]['x_relative_dist']) data.longitude_dist = min(abs(ego_df[ego_df['v'] <= 0.2]['x_relative_dist']), default=0) stop_time = ego_df[abs(ego_df['x_relative_dist']) == min(longitude_dist)]['simTime'].tolist() data.stop_time = min(stop_time) if len(longitude_dist) == 0: return {"maxLongitudeDist_LST": -1} generate_function_chart_data(data, 'maxLongitudeDist_LST', plot_path) return {"maxLongDist_LST": min(longitude_dist)} def noEmergencyBraking_LST(data, plot_path): ego_df = data.ego_data ego_df['ip_dec'] = ego_df['v'].apply( get_interpolation, point1=[18, -5], point2=[72, -3.5]) ego_df['slam_brake'] = (ego_df['accleX'] - ego_df['ip_dec']).apply( lambda x: 1 if x < 0 else 0) if sum(ego_df['slam_brake']) == 0: return {"noEmergencyBraking_LST": 1} else: return {"noEmergencyBraking_LST": -1} def rightWarningSignal_PGVIL(data_processed, plot_path) -> dict: """判断是否发出正确预警信号""" ego_df = data_processed.ego_data scenario_name = find_nested_name(data_processed.function_config["function"]) correctwarning = scenario_sign_dict[scenario_name] if correctwarning is None: print("无法获取正确的预警信号标志位！") return None # 找出本行 correctwarning 和 ifwarning 相等，且 correctwarning 不是 NaN 的行 warning_rows = ego_df[ (ego_df["ifwarning"] == correctwarning) & (ego_df["ifwarning"].notna()) ] if warning_rows.empty: return {"rightWarningSignal_PGVIL": -1} else: return {"rightWarningSignal_PGVIL": 1} def latestWarningDistance_PGVIL(data_processed, plot_path) -> dict: """预警距离计算流水线""" ego_df = data_processed.ego_data obj_df = data_processed.object_df warning_data = get_first_warning(data_processed) if warning_data is None: return {"latestWarningDistance_PGVIL": -11} dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] # dis_pos2head = 5.929 / 2 + 1.982 ego, obj = extract_ego_obj(warning_data) distances = calculate_distance_PGVIL( np.array([[ego["posX"], ego["posY"]]]), obj[["posX", "posY"]].values ) - dis_pos2head # 将计算结果保存到data对象中，供图表生成使用 data_processed.warning_dist = distances data_processed.warning_time = ego['simTime'].tolist() if distances.size == 0: print("没有找到数据！") return {"latestWarningDistance_PGVIL": -1} # 或返回其他默认值，如0.0 generate_function_chart_data(data_processed, 'latestWarningDistance_PGVIL', plot_path) return {"latestWarningDistance_PGVIL": float(np.min(distances))} def latestWarningDistancetoConflictPoint_PGVIL(data_processed, plot_path) -> dict: """预警距离计算流水线""" ego_df = data_processed.ego_data warning_data = get_first_warning(data_processed) if warning_data is None: return {"latestWarningDistancetoConflictPoint_PGVIL": -11} dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] ego, obj = extract_ego_obj(warning_data) ConflictPoint = obj[obj["type"] == 25].copy() distances = calculate_distance_PGVIL( np.array([[ego["posX"], ego["posY"]]]), ConflictPoint[["posX", "posY"]].values ) - dis_pos2head # 将计算结果保存到data对象中，供图表生成使用 data_processed.warning_dist = distances data_processed.warning_time = ego['simTime'].tolist() if distances.size == 0: print("没有找到数据！") return {"latestWarningDistancetoConflictPoint_PGVIL": -1} # 或返回其他默认值，如0.0 generate_function_chart_data(data_processed, 'latestWarningDistancetoConflictPoint_PGVIL', plot_path) return {"latestWarningDistancetoConflictPoint_PGVIL": float(np.min(distances))} def latestWarningDistance_TTC_PGVIL(data_processed, plot_path) -> dict: """TTC计算流水线""" ego_df = data_processed.ego_data obj_df = data_processed.object_df dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] # dis_pos2head = 5.929 / 2 + 1.982 warning_data = get_first_warning(data_processed) if warning_data is None: return {"latestWarningDistance_TTC_PGVIL": 0.0} ego, obj = extract_ego_obj(warning_data) # 向量化计算 ego_pos = np.array([[ego["posX"], ego["posY"]]]) ego_speed = np.array([[ego["speedX"], ego["speedY"]]]) obj_pos = obj[["posX", "posY"]].values obj_speed = obj[["speedX", "speedY"]].values distances = calculate_distance_PGVIL(ego_pos, obj_pos) - dis_pos2head rel_speeds = calculate_relative_speed_PGVIL(ego_speed, obj_speed) data_processed.warning_dist = distances data_processed.warning_speed = rel_speeds data_processed.warning_time = ego['simTime'].tolist() with np.errstate(divide="ignore", invalid="ignore"): ttc = np.where(rel_speeds != 0, distances / rel_speeds, np.inf) if ttc.size == 0: print("没有找到数据！") return {"latestWarningDistance_TTC_PGVIL": -2} # 或返回其他默认值，如0.0 data_processed.ttc = ttc generate_function_chart_data(data_processed, 'latestWarningDistance_TTC_PGVIL', plot_path) return {"latestWarningDistance_TTC_PGVIL": float(np.nanmin(ttc))} def latestWarningDistancetoConflictPoint_TTC_PGVIL(data_processed, plot_path) -> dict: """TTC计算流水线""" ego_df = data_processed.ego_data warning_data = get_first_warning(data_processed) if warning_data is None: return {"latestWarningDistancetoConflictPoint_TTC_PGVIL": -11} dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] ego, obj = extract_ego_obj(warning_data) # 向量化计算 ego_pos = np.array([[ego["posX"], ego["posY"]]]) ego_speed = np.array([[ego["speedX"], ego["speedY"]]]) ConflictPoint = obj[obj["type"] == 25].copy() if ConflictPoint.empty: print("未找到 type == 25 的目标物") return {"latestWarningDistancetoConflictPoint_TTC_PGVIL": -22} ConflictPoint_pos = ConflictPoint[["posX", "posY"]].values ConflictPoint_speed = obj[["speedX", "speedY"]].values distances = calculate_distance_PGVIL(ego_pos, ConflictPoint_pos) - dis_pos2head rel_speeds = calculate_relative_speed_PGVIL(ego_speed, ConflictPoint_speed) data_processed.warning_dist = distances data_processed.warning_speed = rel_speeds data_processed.warning_time = ego['simTime'].tolist() with np.errstate(divide="ignore", invalid="ignore"): ttc = np.where(rel_speeds != 0, distances / rel_speeds, np.inf) if ttc.size == 0: print("没有找到数据！") return {"latestWarningDistancetoConflictPoint_TTC_PGVIL": -33} # 或返回其他默认值，如0.0 data_processed.ttc = ttc generate_function_chart_data(data_processed, 'latestWarningDistancetoConflictPoint_TTC_PGVIL', plot_path) return {"latestWarningDistancetoConflictPoint_TTC_PGVIL": float(np.nanmin(ttc))} def earliestWarningDistance_PGVIL(data_processed, plot_path) -> dict: """预警距离计算流水线""" ego_df = data_processed.ego_data obj_df = data_processed.object_df warning_data = get_first_warning(data_processed) if warning_data is None: return {"earliestWarningDistance_PGVIL": -11} dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] # dis_pos2head = 5.929 / 2 + 1.982 ego, obj = extract_ego_obj(warning_data) distances = calculate_distance_PGVIL( np.array([[ego["posX"], ego["posY"]]]), obj[["posX", "posY"]].values ) - dis_pos2head # 将计算结果保存到data对象中，供图表生成使用 data_processed.warning_dist = distances data_processed.warning_time = ego['simTime'].tolist() if distances.size == 0: print("没有找到数据！") return {"earliestWarningDistance_PGVIL": -22} # 或返回其他默认值，如0.0 generate_function_chart_data(data_processed, 'earliestWarningDistance_PGVIL', plot_path) return {"earliestWarningDistance_PGVIL": float(np.min(distances))} def earliestWarningDistancetoConflictPoint_PGVIL(data_processed, plot_path) -> dict: """预警距离计算流水线""" ego_df = data_processed.ego_data warning_data = get_first_warning(data_processed) if warning_data is None: return {"earliestWarningDistancetoConflictPoint_PGVIL": -11} dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] ego, obj = extract_ego_obj(warning_data) distances = calculate_distance_PGVIL( np.array([[ego["posX"], ego["posY"]]]), obj[["posX", "posY"]].values ) - dis_pos2head # 将计算结果保存到data对象中，供图表生成使用 data_processed.warning_dist = distances data_processed.warning_time = ego['simTime'].tolist() if distances.size == 0: print("没有找到数据！") return {"earliestWarningDistancetoConflictPoint_PGVIL": -22} # 或返回其他默认值，如0.0 generate_function_chart_data(data_processed, 'earliestWarningDistancetoConflictPoint_PGVIL', plot_path) return {"earliestWarningDistancetoConflictPoint_PGVIL": float(np.min(distances))} def earliestWarningDistance_TTC_PGVIL(data_processed, plot_path) -> dict: """TTC计算流水线""" ego_df = data_processed.ego_data obj_df = data_processed.object_df warning_data = get_first_warning(data_processed) if warning_data is None: return {"earliestWarningDistance_TTC_PGVIL": -11} dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] # dis_pos2head = 5.929 / 2 + 1.982 ego, obj = extract_ego_obj(warning_data) # 向量化计算 ego_pos = np.array([[ego["posX"], ego["posY"]]]) ego_speed = np.array([[ego["speedX"], ego["speedY"]]]) obj_pos = obj[["posX", "posY"]].values obj_speed = obj[["speedX", "speedY"]].values distances = calculate_distance_PGVIL(ego_pos, obj_pos) - dis_pos2head rel_speeds = calculate_relative_speed_PGVIL(ego_speed, obj_speed) data_processed.warning_dist = distances data_processed.warning_speed = rel_speeds data_processed.warning_time = ego['simTime'].tolist() with np.errstate(divide="ignore", invalid="ignore"): ttc = np.where(rel_speeds != 0, distances / rel_speeds, np.inf) if ttc.size == 0: print("没有找到数据！") return {"earliestWarningDistance_TTC_PGVIL": -22} # 或返回其他默认值，如0.0 data_processed.ttc = ttc generate_function_chart_data(data_processed, 'earliestWarningDistance_TTC_PGVIL', plot_path) return {"earliestWarningDistance_TTC_PGVIL": float(np.nanmin(ttc))} def earliestWarningDistancetoConflictPoint_TTC_PGVIL(data_processed, plot_path) -> dict: """TTC计算流水线""" ego_df = data_processed.ego_data warning_data = get_first_warning(data_processed) if warning_data is None: return {"earliestWarningDistancetoConflictPoint_TTC_PGVIL": -11} dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] ego, obj = extract_ego_obj(warning_data) ConflictPoint = obj[obj["type"] == 25].copy() if ConflictPoint.empty: print("未找到 type == 25 的目标物") return {"latestWarningDistancetoConflictPoint_TTC_PGVIL": -22} # 向量化计算 ego_pos = np.array([[ego["posX"], ego["posY"]]]) ego_speed = np.array([[ego["speedX"], ego["speedY"]]]) ConflictPoint_pos = ConflictPoint[["posX", "posY"]].values ConflictPoint_speed = ConflictPoint[["speedX", "speedY"]].values distances = calculate_distance_PGVIL(ego_pos, ConflictPoint_pos) - dis_pos2head rel_speeds = calculate_relative_speed_PGVIL(ego_speed, ConflictPoint_speed) data_processed.warning_dist = distances data_processed.warning_speed = rel_speeds data_processed.warning_time = ego['simTime'].tolist() with np.errstate(divide="ignore", invalid="ignore"): ttc = np.where(rel_speeds != 0, distances / rel_speeds, np.inf) if ttc.size == 0: print("没有找到数据！") return {"earliestWarningDistancetoConflictPoint_TTC_PGVIL": -22} # 或返回其他默认值，如0.0 data_processed.ttc = ttc generate_function_chart_data(data_processed, 'earliestWarningDistancetoConflictPoint_TTC_PGVIL', plot_path) return {"earliestWarningDistancetoConflictPoint_TTC_PGVIL": float(np.nanmin(ttc))} def warningDelayTimeofReachDecel_PGVIL(data_processed, plot_path) -> dict: # 预警时机相对背景车辆减速度达到-4m/s2后的时延 ego_df = data_processed.ego_data obj_df = data_processed.object_df warning_data = get_first_warning(data_processed) if warning_data is None: return {"warningDelayTimeofReachDecel_PGVIL": -1} try: ego, _ = extract_ego_obj(warning_data) warning_time = ego["simTime"] if isinstance(ego, pd.Series) else ego["simTime"].iloc[0] # 回溯：只取在预警前的背景车轨迹 obj_pre = obj_df[(obj_df["playerId"] == 2) & (obj_df["simTime"] < warning_time)].copy() if len(obj_pre) < 2: print("预警前目标车辆轨迹不足2帧，无法计算减速度") return {"warningDelayTimeofReachDecel_PGVIL": -2} obj_pre = obj_pre.sort_values(by="simTime").reset_index(drop=True) obj_speed = obj_pre[["speedX", "speedY"]].values simtime_gap = obj_pre["simTime"].iloc[1] - obj_pre["simTime"].iloc[0] obj_speed_magnitude = np.linalg.norm(obj_speed, axis=1) obj_deceleration = np.diff(obj_speed_magnitude) / simtime_gap if np.min(obj_deceleration) < -4: max_index = np.argmin(obj_deceleration) decel_time = obj_pre["simTime"].iloc[max_index + 1] # 注意 +1 是因为 diff 少1行 delay_time = warning_time - decel_time return {"warningDelayTimeofReachDecel_PGVIL": round(float(delay_time), 3)} else: print("未发现背景车减速度低于 -4 m/s² 的时刻") return {"warningDelayTimeofReachDecel_PGVIL": -3} except Exception as e: print("异常:", e) return {"warningDelayTimeofReachDecel_PGVIL": -99} def warningDelayTime_PGVIL(data_processed, plot_path) -> dict: """车端接收到预警到HMI发出预警的时延""" ego_df = data_processed.ego_data # #打印ego_df的列名 # print(ego_df.columns.tolist()) warning_data = get_first_warning(data_processed) if warning_data is None: return {"warningDelayTime_PGVIL": -1} try: ego, obj = extract_ego_obj(warning_data) # 找到event_Type不为空，且playerID为1的行 rosbag_warning_rows = ego_df[(ego_df["event_Type"].notna())] first_time = rosbag_warning_rows["simTime"].iloc[0] warning_time = warning_data[warning_data["playerId"] == 1]["simTime"].iloc[0] delay_time = warning_time - first_time return {"warningDelayTime_PGVIL": float(delay_time)} except Exception as e: print(f"计算预警时延时发生错误: {e}") return {"warningDelayTime_PGVIL": -1} def noWarning_PGVIL(data_processed, plot_path) -> dict: scenario_name = find_nested_name(data_processed.function_config["function"]) correctwarning = scenario_sign_dict[scenario_name] ego_df = data_processed.ego_data if ego_df['ifwarning'].empty: print("无法获取正确预警信号标志位！") return warning_rows = (ego_df['ifwarning'].dropna() == -1).all() if warning_rows: return {"noWarning_PGVIL": 1} else: return {"noWarning_PGVIL": -1} def get_car_to_stop_line_distance(ego, car_point, stop_line_points): """ 计算主车后轴中心点到停止线的距离 :return 距离 """ distance_carpoint_carhead = ego["dimX"] / 2 + ego["offX"] # 计算停止线的方向向量 line_vector = np.array( [ stop_line_points[1][0] - stop_line_points[0][0], stop_line_points[1][1] - stop_line_points[0][1], ] ) direction_vector_norm = np.linalg.norm(line_vector) direction_vector_unit = ( line_vector / direction_vector_norm if direction_vector_norm != 0 else np.array([0, 0]) ) # 计算主车后轴中心点到停止线投影的坐标（垂足） projection_length = np.dot(car_point - stop_line_points[0], direction_vector_unit) perpendicular_foot = stop_line_points[0] + projection_length * direction_vector_unit # 计算主车后轴中心点到垂足的距离 distance_to_foot = np.linalg.norm(car_point - perpendicular_foot) carhead_distance_to_foot = distance_to_foot - distance_carpoint_carhead return carhead_distance_to_foot def ifCrossingRedLight_PGVIL(data_processed, plot_path) -> dict: GREEN = 0x100000 # 1048576 YELLOW = 0x1000000 # 16777216 RED = 0x10000000 # 268435456 ego_df = data_processed.ego_data.copy() stopline_df = data_processed.object_df.copy() # 过滤出停止线对象（type==25），只保留两端点 stopline_df = stopline_df.loc[ stopline_df["type"] == 25, ["simTime", "playerId", "posX", "posY"] ] # 去除无效stateMask帧 ego_df = ego_df[ego_df["stateMask"].notna()].copy() ego_df.sort_values(by="simTime", inplace=True) dis_pos2head = ego_df["dimX"].iloc[0] / 2 + ego_df["offX"].iloc[0] red_light_violation = False prev_distance = float("inf") for index, ego in ego_df.iterrows(): simTime = ego["simTime"] car_point = (ego["posX"], ego["posY"]) stateMask = int(ego["stateMask"]) # 获取该时刻的两个停止线点（playerId 2 和 3） p1_row = stopline_df[(stopline_df["playerId"] == 2) & (stopline_df["simTime"] == simTime)] p2_row = stopline_df[(stopline_df["playerId"] == 3) & (stopline_df["simTime"] == simTime)] # 如果缺失，跳过该帧 if p1_row.empty or p2_row.empty: continue p1 = (p1_row["posX"].iloc[0], p1_row["posY"].iloc[0]) p2 = (p2_row["posX"].iloc[0], p2_row["posY"].iloc[0]) stop_line_points = np.array([p1, p2], dtype=float) distance_to_stopline = get_car_to_stop_line_distance( ego, car_point, stop_line_points ) - dis_pos2head # 如果车头刚刚越线 if prev_distance > 0 and distance_to_stopline <= 0: # 如果不是绿灯，就视为闯红灯 if stateMask != GREEN: red_light_violation = True print(f"[Red Light Violation] simTime={simTime}, stateMask={stateMask}") break prev_distance = distance_to_stopline return {"ifCrossingRedLight_PGVIL": -1 if red_light_violation else 1} def limitSpeed_PGVIL(data, plot_path): ego_df = data.ego_data max_speed = max(ego_df["v"]) if len(ego_df["v"]) == 0: return {"speedLimit_PGVIL": -1} else: return {"speedLimit_PGVIL": round(max_speed * 3.6, 3)} def leastDistance_PGVIL(data, plot_path): exclude_seconds = 2.0 ego_df = data.ego_data stopline_df = data.object_df stopline_df = stopline_df.loc[ stopline_df["type"] == 25, ["simTime", "playerId", "posX", "posY"] ] max_sim_time = ego_df["simTime"].max() threshold_time = max_sim_time - exclude_seconds for index, ego in ego_df.iterrows(): if ego["simTime"] >= threshold_time: continue if "v" in ego and ego["v"] == 0: current_time = ego["simTime"] car_point = (ego["posX"], ego["posY"]) # p1 = (ego["stopline_x1"], ego["stopline_y1"]) # p2 = (ego["stopline_x2"], ego["stopline_y2"]) p1_row = stopline_df[(stopline_df["playerId"] == 2) & (stopline_df["simTime"] == current_time)] p2_row = stopline_df[(stopline_df["playerId"] == 3) & (stopline_df["simTime"] == current_time)] if p1_row.empty or p2_row.empty: continue # 如果找不到，就跳过，继续找下一个停车帧 p1 = (p1_row["posX"].iloc[0], p1_row["posY"].iloc[0]) p2 = (p2_row["posX"].iloc[0], p2_row["posY"].iloc[0]) stop_line_points = np.array([p1, p2], dtype=float) distance_to_stopline = get_car_to_stop_line_distance( ego, car_point, stop_line_points ) # print(f"distance_to_stopline:{distance_to_stopline}") return {"leastDistance_PGVIL": round(distance_to_stopline, 3)} return {"leastDistance_PGVIL": -1} def launchTimeinStopLine_PGVIL(data, plot_path): ego_df = data.ego_data # === 先计算有效时间段 === max_sim_time = ego_df["simTime"].max() min_sim_time = ego_df["simTime"].min() threshold_start = min_sim_time + 3.0 threshold_end = max_sim_time - 3.0 # === 只保留有效时间段 === ego_df = ego_df[(ego_df["simTime"] >= threshold_start) & (ego_df["simTime"] <= threshold_end)] in_stop = False start_time = None last_low_time = None max_duration = 0.0 v_thresh = 0.01 for _, row in ego_df.iterrows(): simt = row["simTime"] v = row.get("v", None) if v is None or np.isnan(v) or abs(v) > v_thresh: if in_stop: duration = last_low_time - start_time if duration > max_duration: max_duration = duration in_stop = False else: if not in_stop: start_time = simt in_stop = True last_low_time = simt if in_stop: duration = last_low_time - start_time if duration > max_duration: max_duration = duration if max_duration <= 0: return {"launchTimeinStopLine_PGVIL": -1} else: return {"launchTimeinStopLine_PGVIL": round(float(max_duration), 3)} def launchTimeinTrafficLight_PGVIL(data, plot_path): GREEN = 1048576 # 0x100000 RED = 268435456 # 0x10000000 ego_df = data.ego_data mask = (ego_df["stateMask"]).notna() ego_df_light = ego_df[mask].copy() ego_df_light = ego_df_light.drop_duplicates(subset=["simTime"]).sort_values(by="simTime") # 找到第一次红灯 to 绿灯的切换时刻 is_transition = ( (ego_df_light["stateMask"] == GREEN) & (ego_df_light["stateMask"].shift(1) == RED) ) transition_times = ego_df_light.loc[is_transition, "simTime"] if transition_times.empty: # print("没有红绿灯切换") return {"launchTimeinTrafficLight_PGVIL": -88} time_red2green = transition_times.iloc[0] car_speed_at_transition = ego_df.loc[ego_df["simTime"] == time_red2green, "v"] if not car_speed_at_transition.empty and car_speed_at_transition.iloc[0] > 0.5: return {"launchTimeinTrafficLight_PGVIL": -66} car_move_times = ego_df.loc[ (ego_df["simTime"] >= time_red2green) & (ego_df["v"] > 0), "simTime" ] if car_move_times.empty: # print("没有车移动") return {"launchTimeinTrafficLight_PGVIL": -99} time_move = car_move_times.iloc[0] return {"launchTimeinTrafficLight_PGVIL": round(time_move - time_red2green, 3)} def crossJunctionToTargetLane_PGVIL(data, plot_path): ego_df = data.ego_data road_ids = set(ego_df["road_id"].dropna()) try: scenario_name = find_nested_name(data.function_config["function"]) target_lane_id = data.function_config["function"][scenario_name][ "crossJunctionToTargetLane_PGVIL" ]["max"] except KeyError: raise ValueError("请在配置文件中指定目标车道ID！") result = target_lane_id if target_lane_id in road_ids else -1 return {"crossJunctionToTargetLane_PGVIL": result} def crossJunctionToTargetLane_PGVIL(data, plot_path): ego_df = data.ego_data road_ids = set(ego_df["road_id"].dropna()) try: scenario_name = find_nested_name(data.function_config["function"]) target_lane_id = data.function_config["function"][scenario_name][ "crossJunctionToTargetLane_PGVIL" ]["max"] except KeyError: raise ValueError("请在配置文件中指定目标车道ID！") result = target_lane_id if target_lane_id in road_ids else -1 return {"crossJunctionToTargetLane_PGVIL": result} def noStop_PGVIL(data, plot_path): exclude_end_seconds = 5.0 exclude_start_seconds = 5.0 ego_df = data.ego_data min_sim_time = ego_df["simTime"].min() max_sim_time = ego_df["simTime"].max() start_threshold = min_sim_time + exclude_start_seconds end_threshold = max_sim_time - exclude_end_seconds filtered_df = ego_df[ (ego_df["simTime"] >= start_threshold) & (ego_df["simTime"] <= end_threshold) ] if (filtered_df["v"] == 0).any(): return {"noStop_PGVIL": -1} else: return {"noStop_PGVIL": 1} def noEmergencyBraking_PGVIL(data, plot_path): ego_df = data.ego_data ego_df["ip_dec"] = ego_df["v"].apply( get_interpolation, point1=[18, -5], point2=[72, -3.5] ) ego_df["slam_brake"] = (ego_df["accelX"] - ego_df["ip_dec"]).apply( lambda x: 1 if x < 0 else 0 ) if sum(ego_df["slam_brake"]) == 0: return {"noEmergencyBraking_PGVIL": 1} else: return {"noEmergencyBraking_PGVIL": -1} def noReverse_PGVIL(data, plot_path): ego_df = data.ego_data.copy() ego_df["posH"] = (90 - ego_df["posH"]) % 360 yaw_rad = np.deg2rad(ego_df["posH"]) # 车头朝向向量 heading_x = np.cos(yaw_rad) heading_y = np.sin(yaw_rad) # 速度向量 speed_x = ego_df["speedX"] speed_y = ego_df["speedY"] # 点积：判断速度是否与车头方向一致 dot_product = speed_x * heading_x + speed_y * heading_y moving = np.hypot(speed_x, speed_y) > 0.5 # 排除近乎静止的帧（避免误判） reverse_flag = (dot_product < 0) & moving # 倒车帧：点积为负 且 在移动 if reverse_flag.any(): return {"noReverse_PGVIL": -1} # 存在倒车 else: return {"noReverse_PGVIL": 1} # 全程未倒车 def laneOffset_PGVIL(data, plot_path): ego_df = data.ego_data car_width = ego_df["dimY"].loc[0] print(f"car_width:{car_width}") is_zero = ego_df["v"] == 0 if not is_zero.any(): # 全程未停车 return {"laneOffset_PGVIL": -1} groups = (is_zero != is_zero.shift(fill_value=False)).cumsum() last_zero_group = groups[is_zero].iloc[-1] last_stop = ego_df[groups == last_zero_group] # 距离右侧车道线 edge_dist = (last_stop["width"] / 2 + last_stop["laneOffset"]) - ( car_width / 2 ) return {"laneOffset_PGVIL": round(edge_dist.max(), 3)} def maxLongitudelDistance_PGVIL(data, plot_path): ego_df = data.ego_data object_df = data.object_df is_zero = ego_df["v"] == 0 if not is_zero.any(): # 全程未停车 return {"maxLongitudelDistance_PGVIL": -1} groups = (is_zero != is_zero.shift(fill_value=False)).cumsum() last_zero_group = groups[is_zero].iloc[-1] last_stop = ego_df[groups == last_zero_group] stop_point = object_df[object_df["type"] == 25] stop_x = stop_point["posX"].iloc[0] stop_y = stop_point["posY"].iloc[0] dx = last_stop["posX"] - stop_x dy = last_stop["posY"] - stop_y last_stop["posH"] = (90 - last_stop["posH"]) % 360 heading_rad = last_stop["posH"] # 弧度制 longitudinal_offset = np.cos(heading_rad) * dx + np.sin(heading_rad) * dy max_longitudinal_offset = np.abs(longitudinal_offset).max() return {"maxLongitudelDistance_PGVIL": round(max_longitudinal_offset, 3)} def keepInLane_PGVIL(data, plot_path): ego_df = data.ego_data.copy() ego_df = ego_df.dropna(subset=["laneOffset", "lane_width"]) if ego_df.empty: return {"keepInLane_PGVIL": -1} threshold = ego_df["lane_width"] / 2 if (ego_df["laneOffset"].abs() > threshold).any(): return {"keepInLane_PGVIL": -1} else: return {"keepInLane_PGVIL": 1} # def launchTimewhenPedestrianLeave_PGVIL(data, plot_path): # ego_df = data.ego_data # object_df = data.object_df # # == 1. 提取行人轨迹 == # ped_df = object_df.loc[object_df["playerId"] == 2, ["simTime", "posX", "posY"]] # merged = ego_df.merge(ped_df, on="simTime", how="inner", suffixes=("", "_ped")) # if merged.empty: # return {"launchTimewhenPedestrianLeave_PGVIL": -1} # # == 2. 判断主车是否在行人完全离开前抢行 == # ego = ego_df[["simTime", "posX", "posY", "posH", "dimX", "offX"]].copy() # ped = ped_df.rename(columns={"posX": "posX_ped", "posY": "posY_ped"}) # ego_ped = pd.merge(ego, ped, on="simTime") # ego_ped = ego_ped.sort_values("simTime").reset_index(drop=True) # front_offset = ego_ped["dimX"] / 2 + ego_ped["offX"] # yaw = np.deg2rad(ego_ped["posH"]) # ego_ped["front_x"] = ego_ped["posX"] + np.cos(yaw) * front_offset # ego_ped["front_y"] = ego_ped["posY"] + np.sin(yaw) * front_offset # dx = ego_ped["front_x"] - ego_ped["posX_ped"] # dy = ego_ped["front_y"] - ego_ped["posY_ped"] # ego_ped["proj"] = dx * np.cos(yaw) + dy * np.sin(yaw) # ego_ped["lateral_dist"] = np.abs(-np.sin(yaw) * dx + np.cos(yaw) * dy) # cross_idx = ego_ped[ego_ped["proj"] > 0].index # if not cross_idx.empty and cross_idx[-1] <= len(ego_ped) - 3: # i = cross_idx[0] # d0 = ego_ped.loc[i, "lateral_dist"] # d1 = ego_ped.loc[i + 1, "lateral_dist"] # d2 = ego_ped.loc[i + 2, "lateral_dist"] # if d1 < d0 and d2 < d1: # return {"launchTimewhenPedestrianLeave_PGVIL": -66} # 主车未避让行人 # # == 3. 判断行人进入与离开车道 == # yaw_rad = np.deg2rad(merged["posH"]) # dx = merged["posX_ped"] - merged["posX"] # dy = merged["posY_ped"] - merged["posY"] # merged["lateral_dist"] = np.abs(-np.sin(yaw_rad) * dx + np.cos(yaw_rad) * dy) # merged["threshold"] = merged["lane_width"] / 2 # entry = merged[merged["lateral_dist"] <= merged["threshold"]] # if entry.empty: # return {"launchTimewhenPedestrianLeave_PGVIL": -11} # t_entry = entry["simTime"].iloc[0] # after_e = merged[merged["simTime"] > t_entry] # leave = after_e[after_e["lateral_dist"] > after_e["threshold"]] # if leave.empty: # return {"launchTimewhenPedestrianLeave_PGVIL": -22} # t_leave = leave["simTime"].iloc[0] # ped_x, ped_y = leave[["posX_ped", "posY_ped"]].iloc[0] # # == 4. 查找 t_leave 之后主车是否在前方合理范围内停车 == # stops = ego_df[(ego_df["simTime"] >= t_leave) & (ego_df["v"] == 0)].copy() # if stops.empty: # return {"launchTimewhenPedestrianLeave_PGVIL": -33} # front_overPos = stops["dimX"] / 2 + stops["offX"] # yaw_stop = np.deg2rad(stops["posH"]) # stops["front_x"] = stops["posX"] + np.cos(yaw_stop) * front_overPos # stops["front_y"] = stops["posY"] + np.sin(yaw_stop) * front_overPos # dx_s = stops["front_x"] - ped_x # dy_s = stops["front_y"] - ped_y # proj = dx_s * np.cos(yaw_stop) + dy_s * np.sin(yaw_stop) # valid = stops[ # (proj >= 0) & # (proj <= 20) & # ((stops["simTime"] - t_leave) <= 5.0) # ] # if valid.empty: # return {"launchTimewhenPedestrianLeave_PGVIL": -88} # # == 5. 查找 t_stop 后首次启动 == # t_stop = valid["simTime"].iloc[0] # launches = ego_df[(ego_df["simTime"] > t_stop) & (ego_df["v"] > 0)]["simTime"] # if launches.empty: # return {"launchTimewhenPedestrianLeave_PGVIL": -99} # t_launch = launches.iloc[0] # return {"launchTimewhenPedestrianLeave_PGVIL": round(t_launch - t_stop, 3)} def launchTimewhenPedestrianLeave_PGVIL(data, plot_path): ego_df = data.ego_data object_df = data.object_df ego_df["posH"] = (90 - ego_df["posH"]) % 360 object_df["posH"] = (90 - object_df["posH"]) % 360 ped_df = object_df.loc[object_df["playerId"] == 2, ["simTime", "posX", "posY"]] merged = ego_df.merge(ped_df, on="simTime", how="inner", suffixes=("", "_ped")) if merged.empty: return {"launchTimewhenPedestrianLeave_PGVIL": -1} yaw_rad = np.deg2rad(merged["posH"]) # 度→弧度 dx = merged["posX_ped"] - merged["posX"] dy = merged["posY_ped"] - merged["posY"] merged["lateral_dist"] = np.abs(-np.sin(yaw_rad) * dx + np.cos(yaw_rad) * dy) merged["threshold"] = merged["lane_width"] / 2 entry = merged[merged["lateral_dist"] <= merged["threshold"]] if entry.empty: return {"launchTimewhenPedestrianLeave_PGVIL": -11} t_entry = entry["simTime"].iloc[0] after_e = merged[merged["simTime"] > t_entry] leave = after_e[after_e["lateral_dist"] > after_e["threshold"]] if leave.empty: return {"launchTimewhenPedestrianLeave_PGVIL": -22} t_leave = leave["simTime"].iloc[0] ped_x, ped_y = leave[["posX_ped", "posY_ped"]].iloc[0] ego_after_leave = ego_df[ego_df["simTime"] >= t_leave].copy() min_V = ego_after_leave["v"].min() if ego_after_leave["v"].min() == 0: stops = ego_after_leave[ego_after_leave["v"] == 0].copy() elif min_V <= 1: stops = ego_after_leave[ego_after_leave["v"] == min_V].copy() else: stops = pd.DataFrame() # stops = ego_df[(ego_df["simTime"] >= t_leave) & (ego_df["v"] <= 0.01)].copy() if stops.empty: # 没有停车 return {"launchTimewhenPedestrianLeave_PGVIL": -33} front_overPos = stops["dimX"] / 2 + stops["offX"] yaw_stop = np.deg2rad(stops["posH"]) stops["front_x"] = stops["posX"] + np.cos(yaw_stop) * front_overPos stops["front_y"] = stops["posY"] + np.sin(yaw_stop) * front_overPos dx_s = stops["front_x"] - ped_x dy_s = stops["front_y"] - ped_y proj = dx_s * np.cos(yaw_stop) + dy_s * np.sin(yaw_stop) valid = stops[ (proj >= 0) & (proj <= 20) & ((stops["simTime"] - t_leave) <= 5.0) ] if valid.empty: # 车辆未在行人离开后停止 return {"launchTimewhenPedestrianLeave_PGVIL": -88} t_stop = valid["simTime"].iloc[0] if ego_after_leave["v"].min() == 0: launches_data = ego_after_leave[ego_after_leave["v"] > 0].copy() else: launches_data = ego_after_leave[ego_after_leave["v"] > 1].copy() launches = launches_data[launches_data["simTime"] > t_stop] # launches = ego_df[(ego_df["simTime"] > t_stop) & (ego_df["v"] > 0.5)]["simTime"] if launches.empty: # 车辆停止后未启动 return {"launchTimewhenPedestrianLeave_PGVIL": -99} # t_launch = launches.iloc[0] t_launch = launches["simTime"].iloc[0] return {"launchTimewhenPedestrianLeave_PGVIL": round(t_launch - t_stop, 3)} def waitTimeAtCrosswalkwithPedestrian_PGVIL(data, plot_path): ego_df = data.ego_data object_df = data.object_df # ego_df["posH"] = (90 - ego_df["posH"]) % 360 # object_df["posH"] = (90 - object_df["posH"]) % 360 ped_df = object_df.loc[object_df["playerId"] == 2, ["simTime", "posX", "posY"]] first_launch = ego_df.loc[ego_df["v"] > 0, "simTime"] if first_launch.empty: t0_launch = 0.0 else: t0_launch = first_launch.iloc[0] merged = ego_df.merge(ped_df, on="simTime", how="inner", suffixes=("", "_ped")) merged["front_offset"] = merged["dimX"] / 2 + merged["offX"] yaw = np.deg2rad(merged["posH"]) merged["front_x"] = merged["posX"] + np.cos(yaw) * merged["front_offset"] merged["front_y"] = merged["posY"] + np.sin(yaw) * merged["front_offset"] dx = merged["posX_ped"] - merged["front_x"] dy = merged["posY_ped"] - merged["front_y"] valid_times = (dx * np.cos(yaw) + dy * np.sin(yaw)) <= 0.1 stops_df = merged.loc[ (merged["simTime"] >= t0_launch) & (merged["v"] == 0) & valid_times ].sort_values("simTime") if stops_df.empty: print("no stopping time found!") return {"waitTimeAtCrosswalkwithPedestrian_PGVIL": -1} wait_time = stops_df["simTime"].iloc[-1] - stops_df["simTime"].iloc[0] return {"waitTimeAtCrosswalkwithPedestrian_PGVIL": round(wait_time, 3)} def noCollision_PGVIL(data, plot_path): ego_df = data.ego_data object_df = data.object_df ego_df['posH'] = (90 - ego_df['posH']) % 360 object_df['posH'] = (90 - object_df['posH']) % 360 ego = ego_df[["simTime", "posX", "posY", "posH", "dimX", "dimY", "offX"]] tar = object_df.loc[object_df["playerId"] == 2, ["simTime", "posX", "posY", "posH", "dimX", "dimY", "offX"]] df = ego.merge(tar, on="simTime", suffixes=("", "_tar")) df["posH_tar_rad"] = np.deg2rad(df["posH_tar"]) df["posH_rad"] = np.deg2rad(df["posH"]) df["collision"] = df.apply(lambda row: funcIsCollision( row["dimX"], row["dimY"], row["offX"], 0, row["posX"], row["posY"], row["posH_rad"], row["dimX_tar"], row["dimY_tar"], row["offX_tar"], 0, row["posX_tar"], row["posY_tar"], row["posH_tar_rad"], ), axis=1, ) if df["collision"].any(): return {"noCollision_PGVIL": -1} else: return {"noCollision_PGVIL": 1} def leastLateralDistance_PGVIL(data, plot_path): ego_df = data.ego_data object_df = data.object_df ego_df['posH'] = (90 - ego_df['posH']) % 360 object_df['posH'] = (90 - object_df['posH']) % 360 cones = object_df[object_df["type"] == 25].copy() ego = ego_df[["simTime", "posX", "posY", "posH", "dimX", "dimY", "offX"]].copy() ego["posH_rad"] = np.deg2rad(ego["posH"]) has_collision = False for idx, cone in cones.iterrows(): # 找到与ego同一时刻的数据 ego_at_time = ego[ego["simTime"] == cone["simTime"]] if ego_at_time.empty: continue posX_tar = cone["posX"] posY_tar = cone["posY"] dimX_tar = cone["dimX"] dimY_tar = cone["dimY"] offX_tar = cone.get("offX", 0) posH_tar_rad = np.deg2rad(cone["posH"]) if not np.isnan(cone["posH"]) else 0 for _, row in ego_at_time.iterrows(): result = funcIsCollision( row["dimX"], row["dimY"], row["offX"], 0, row["posX"], row["posY"], row["posH_rad"], dimX_tar, dimY_tar, offX_tar, 0, posX_tar, posY_tar, posH_tar_rad, ) if result: has_collision = True break if has_collision: break return {"leastLateralDistance_PGVIL": -1 if has_collision else 1} class FunctionRegistry: """动态函数注册器（支持参数验证）""" def __init__(self, data_processed, plot_path): self.logger = LogManager().get_logger() # 获取全局日志实例 self.data = data_processed self.plot_path = plot_path # 检查function_config是否为空 if not hasattr(data_processed, 'function_config') or not data_processed.function_config: self.logger.warning("功能配置为空，跳过功能指标计算") self.fun_config = {} self.level_3_merics = [] self._registry = {} return self.fun_config = data_processed.function_config.get("function", {}) self.level_3_merics = self._extract_level_3_metrics(self.fun_config) self._registry: Dict[str, Callable] = {} self._registry = self._build_registry() def _extract_level_3_metrics(self, config_node: dict) -> list: """DFS遍历提取第三层指标（时间复杂度O(n)）[4](@ref)""" metrics = [] def _recurse(node): if isinstance(node, dict): if "name" in node and not any( isinstance(v, dict) for v in node.values() ): metrics.append(node["name"]) for v in node.values(): _recurse(v) _recurse(config_node) self.logger.info(f"评比的功能指标列表:{metrics}") return metrics def _build_registry(self) -> dict: """自动注册指标函数（防御性编程）""" registry = {} for func_name in self.level_3_merics: try: registry[func_name] = globals()[func_name] except KeyError: print(f"未实现指标函数: {func_name}") self.logger.error(f"未实现指标函数: {func_name}") return registry def batch_execute(self) -> dict: """批量执行指标计算（带熔断机制）""" results = {} # 如果配置为空或没有注册的指标，直接返回空结果 if not hasattr(self, 'fun_config') or not self.fun_config or not self._registry: self.logger.info("功能配置为空或无注册指标，返回空结果") return results for name, func in self._registry.items(): try: result = func(self.data, self.plot_path) # 统一传递数据上下文 results.update(result) except Exception as e: print(f"{name} 执行失败: {str(e)}") self.logger.error(f"{name} 执行失败: {str(e)}", exc_info=True) results[name] = None self.logger.info(f"功能指标计算结果:{results}") return results class FunctionManager: """管理功能指标计算的类""" def __init__(self, data_processed, plot_path): self.data = data_processed self.logger = LogManager().get_logger() self.plot_path = plot_path # 检查function_config是否为空 if not hasattr(data_processed, 'function_config') or not data_processed.function_config: self.logger.warning("功能配置为空，跳过功能指标计算初始化") self.function = None else: self.function = FunctionRegistry(self.data, self.plot_path) def report_statistic(self): """ 计算并报告功能指标结果。 :return: 评估结果 """ # 如果function为None，直接返回空字典 if self.function is None: self.logger.info("功能指标管理器未初始化，返回空结果") return {} function_result = self.function.batch_execute() print("\n[功能性表现及评价结果]") return function_result # self.logger.info(f'Function Result: {function_result}') # 使用示例 if __name__ == "__main__": pass # print("\n[功能类表现及得分情况]")