zhaoyuan_DataPreProcess/core/processors/built_in/pgvil.py

from pathlib import Path
from typing import Dict, Any, Optional
import pandas as pd
import numpy as np
from pyproj import Proj
from dataclasses import dataclass, field
from typing import Dict, Optional
from pathlib import Path
import pandas as pd
from core.error_handler import ErrorHandler
from core.config_manager import get_config
import sys
import csv
import os
import zipfile
import argparse
from genpy import Message
import shutil
import tempfile                                                                                              
import pandas as pd
import subprocess
import pandas as pd
import numpy as np

@dataclass
class Config:
    """PGVIL处理器配置类"""

    zip_path: Path
    output_path: Path
    engine_path: Optional[Path] = None
    map_path: Optional[Path] = None
    utm_zone: int = 51  # Example UTM zone
    x_offset: float = 0.0
    y_offset: float = 0.0

    def __post_init__(self):
        # Use output_path directly as output_dir to avoid nested directories
        self.output_dir = self.output_path
        self.output_dir.mkdir(parents=True, exist_ok=True)

def run_pgvil_engine(config: Config):
    """Runs the external C++ preprocessing engine."""
    if not config.engine_path or not config.map_path:
        print("C++ engine path or map path not configured. Skipping C++ engine execution.")
        return True  # Return True assuming it's optional or handled elsewhere

    engine_cmd = [
        str(config.engine_path),
        str(config.map_path),
        str(config.output_dir),
        str(config.x_offset),
        str(config.y_offset)
    ]

    print(f"--- Running C++ Preprocessing Engine ---")
    print(f"Command: {' '.join(engine_cmd)}")

    try:
        result = subprocess.run(
            engine_cmd,
            check=True,  # Raise exception on non-zero exit code
            capture_output=True,  # Capture stdout/stderr
            text=True,  # Decode output as text
            cwd=config.engine_path.parent  # Run from the engine's directory? Or script's? Adjust if needed.
        )
        print("C++ Engine Output:")
        print(result.stdout)
        if result.stderr:
            print("C++ Engine Error Output:")
            print(result.stderr)
        print("--- C++ Engine Finished Successfully ---")
        return True
    except FileNotFoundError:
        print(f"Error: C++ engine executable not found at {config.engine_path}.")
        return False
    except subprocess.CalledProcessError as e:
        print(f"Error: C++ engine failed with exit code {e.returncode}.")
        print("C++ Engine Output (stdout):")
        print(e.stdout)
        print("C++ Engine Output (stderr):")
        print(e.stderr)
        return False
    except Exception as e:
        print(f"An unexpected error occurred while running the C++ engine: {e}")
        return False


def remove_conflicting_columns(df_object, df_csv_info):
    """
    delete the columns that are in both dataframes and are not simTime, simFrame, or playerId
    """
    conflicting_columns = set(df_object.columns) & set(df_csv_info.columns)
    for col in conflicting_columns:
        if col not in ["simTime", "simFrame", "playerId"]:
            del df_csv_info[col]
    return df_csv_info


def align_simtime_by_simframe(df):
    # 创建一个映射，将simFrame映射到其对应的simTime代表值
    sim_frame_to_time_map = df.groupby('simFrame')['simTime'].first().to_dict()
    
    # 使用映射来更新DataFrame中的simTime值
    df['simTime'] = df['simFrame'].map(sim_frame_to_time_map)
    # 检查simFrame列是否为空或包含非整数类型的数据
    if df['simFrame'].empty or not df['simFrame'].apply(lambda x: isinstance(x, (int, np.integer))).all():
        return df

    # 识别缺失的simFrame
    all_frames = np.arange(df['simFrame'].min(), df['simFrame'].max() + 1)
    missing_frames = set(all_frames) - set(df['simFrame'])
    new_rows = []
    # 填补缺失的simFrame
    for missing_frame in missing_frames:
        prev_frame = df[df['simFrame'] < missing_frame]['simFrame'].max()
        next_frame = df[df['simFrame'] > missing_frame]['simFrame'].min()
        
        if prev_frame is not None and next_frame is not None:
            prev_row = df[df['simFrame'] == prev_frame].iloc[0]
            next_row = df[df['simFrame'] == next_frame].iloc[0]
            
            # 计算平均值并创建新行
            new_row = prev_row.copy()
            new_row['simFrame'] = missing_frame
            for col in df.columns:
                if col not in ['simTime', 'simFrame']:
                    new_row[col] = (prev_row[col] + next_row[col]) / 2
            
            # 更新simTime值
            new_row['simTime'] = sim_frame_to_time_map.get(missing_frame, np.nan)
            
            # 将新行添加到DataFrame中
            new_rows.append(new_row)
    if new_rows:
        df = pd.concat([df, pd.DataFrame(new_rows)], ignore_index=True)
    return df.sort_values(by='simFrame').reset_index(drop=True)

def mergecopy_by_simtime(merged_df, external_df,ignore_cols,prefix=None,):
    """
    将external_df中所有的字段，基于nearest simTime,批量合并到merged_df中simtime相同的所有行中
    """
    useful_cols = [col for col in external_df.columns if col not in ignore_cols]
    for col in useful_cols:
        col_name = f"{prefix}_{col}" if prefix else col
        mapping = external_df.set_index('nearest_simTime')[col].to_dict()
        merged_df[col_name] = merged_df['simTime'].map(mapping)
    return merged_df

def read_csv_with_filled_columns(file_path):
    try:
        # 确保 file_path 是字符串类型且有效
        if not isinstance(file_path, str):
            raise ValueError("提供的文件路径无效")
        if not os.path.exists(file_path):
            raise FileNotFoundError(f"文件 {file_path} 不存在")
        
        # 使用 on_bad_lines='skip' 跳过格式错误的行
        df = pd.read_csv(file_path, on_bad_lines='skip')  # 跳过格式错误的行
        
        # 强制填充缺失的列为 NaN，确保列数一致
        if not df.empty:  # 确保 df 不为空
            df.fillna(np.nan, inplace=True)  # 用 NaN 填充所有空值
        return df
    except Exception as e:
        print(f"读取 CSV 文件 {file_path} 时发生错误: {str(e)}")
        return pd.DataFrame()  # 返回空的 DataFrame 以便继续处理

def convert_heading(posH_rad):
    # 将弧度转换为角度
    angle_deg = np.degrees(posH_rad)
    # 逆时针东为0 => 顺时针北为0，相当于 new_angle = (90 - angle_deg) % 360
    heading_deg = (90 - angle_deg) % 360
    return round(heading_deg,3)


class PGVILProcessor:
    """PGVIL数据处理器，实现PGVIL特有的处理逻辑"""
    
    def __init__(self, config: Config):
        self.config = config

    def process_zip(self) -> Path:
        """处理输入ZIP文件，并返回输出目录路径
        zip_path
        output_dir
        """
        print(f"Processing ZIP: {self.config.zip_path}")
        zip_path = self.config.zip_path
        output_dir = Path(self.config.output_dir)#将目录路径转换为Path对象

        # 创建以 ZIP 名称为子目录的提取目录
        zip_name = Path(zip_path).stem
        output_dir.mkdir(parents=True, exist_ok=True)

        # 提取HMIdata和RDBdata中的CSV文件
        with zipfile.ZipFile(zip_path, 'r') as zip_ref:
            for name in zip_ref.namelist():
                if ('HMIdata/' in name or 'RDBdata/' in name) and name.endswith('.csv'):
                    # 原 zip 内的子路径最后一部分为文件名
                    filename = os.path.basename(name)
                    src = zip_ref.open(name)
                    dst_path = output_dir / filename
                    # print(f"提取 {name} 到 {dst_path}")
                    with open(dst_path, 'wb') as dst_file:
                        shutil.copyfileobj(src, dst_file)
        # 更新 config 中的输出目录为刚才的子目录
        self.config.output_dir = output_dir
        return output_dir


    def merge_csv_files(self):
        x_offset = self.config.x_offset
        y_offset = self.config.y_offset
        data_path = self.config.output_dir
        # X_OFFSET = 258109.4239876
        # Y_OFFSET = 4149969.964821

        # 定义CSV文件路径       
        try:
            obj_state_path = os.path.join(data_path, "ObjState.csv")
            ego_map_path = os.path.join(data_path, "EgoMap.csv")
            lane_map_path = os.path.join(data_path, "LaneMap.csv")
            laneINfo_path = os.path.join(data_path, "LaneInfo.csv")
            roadPos_path = os.path.join(data_path, "RoadPos.csv")
            vehicleystems_path = os.path.join(data_path, "VehicleSystems.csv")
            trafficlight_path = os.path.join(data_path, "TrafficLight.csv")
            function_path = os.path.join(data_path, "Function.csv")
        except FileNotFoundError:
            raise Exception("File not found")
        
        df_object = read_csv_with_filled_columns(obj_state_path) 
        df_map_info = read_csv_with_filled_columns(ego_map_path) 
        df_lane_map = read_csv_with_filled_columns(lane_map_path) 
        df_laneINfo = read_csv_with_filled_columns(laneINfo_path) 
        df_roadPos = read_csv_with_filled_columns(roadPos_path) 
        df_vehicleystems = read_csv_with_filled_columns(vehicleystems_path)
        df_trafficlight = read_csv_with_filled_columns(trafficlight_path)
        df_function = read_csv_with_filled_columns(function_path)

        # 检查并转换数值型列
        def convert_numeric_columns(df):
            numeric_cols = df.select_dtypes(include=['number']).columns
            df[numeric_cols] = df[numeric_cols].astype(float)
            return df

        df_object = convert_numeric_columns(df_object)
        df_map_info = convert_numeric_columns(df_map_info)
        df_lane_map = convert_numeric_columns(df_lane_map)
        df_laneINfo = convert_numeric_columns(df_laneINfo)
        df_roadPos = convert_numeric_columns(df_roadPos)
        df_vehicleystems = convert_numeric_columns(df_vehicleystems)
        df_trafficlight = convert_numeric_columns(df_trafficlight)
        df_function = convert_numeric_columns(df_function)


        # 对df_object中的posX和posY应用偏置
        df_object['posX'] += x_offset
        df_object['posY'] += y_offset

        # 对齐simTime和simFrame
        df_object = align_simtime_by_simframe(df_object)
        df_map_info = align_simtime_by_simframe(df_map_info)
        df_lane_map = align_simtime_by_simframe(df_lane_map)
        df_laneINfo = align_simtime_by_simframe(df_laneINfo)
        df_roadPos = align_simtime_by_simframe(df_roadPos)
        df_vehicleystems = align_simtime_by_simframe(df_vehicleystems)
        df_trafficlight = align_simtime_by_simframe(df_trafficlight)
        
        del_ego_map = remove_conflicting_columns(df_object, df_map_info)#去掉重复的列
        # 合并数据
        merged_df = pd.merge(df_object, del_ego_map, on=["simTime", "simFrame", "playerId"], how="left")


        # 使用simTime, simFrame, playerId合并ObjState和LaneMap\trafficlight\trafficlight
        del_lane_map = remove_conflicting_columns(merged_df, df_lane_map)
        merged_df = pd.merge(merged_df, del_lane_map, on=["simTime", "simFrame", "playerId"], how="left").drop_duplicates()
        del_laneINfo = remove_conflicting_columns(merged_df, df_laneINfo)
        merged_df = pd.merge(merged_df, del_laneINfo, on=["simTime", "simFrame", "playerId"], how="left").drop_duplicates()
        del_roadPos = remove_conflicting_columns(merged_df, df_roadPos)
        merged_df = pd.merge(merged_df, del_roadPos, on=["simTime", "simFrame", "playerId"], how="left").drop_duplicates()
        del_trafficlight = remove_conflicting_columns(merged_df, df_trafficlight)
        merged_df = pd.merge(merged_df, del_trafficlight, on=["simTime", "simFrame"], how="left").drop_duplicates()
        del_vehicleystems = remove_conflicting_columns(merged_df, df_vehicleystems)
        merged_df = pd.merge(merged_df, del_vehicleystems, on=["simTime", "simFrame", "playerId"], how="left").drop_duplicates()
        
        tolerance = 0.01
        def find_closest_time(sim_time, sim_time_to_index, tolerance=0.01):
            # 找到最接近的时间点，并且该时间点的差异小于 tolerance
            closest_time = min(sim_time_to_index.keys(), key=lambda y: abs(y - sim_time) if abs(y - sim_time) < tolerance else float('inf'))
            return closest_time

        #创建一个映射，存储 df_object 中每个 simTime 值及其对应的行索引
        sim_time_to_index = {row['simTime']: idx for idx, row in merged_df.iterrows()}

        df_function = df_function.sort_values(by='simTime').reset_index(drop=True)#按simTime列排序
        #找到 function.csv 中每个 simTime 值在 df_object 中的最近时间点
        df_function['nearest_simTime'] = df_function['simTime'].apply(lambda x: find_closest_time(x, sim_time_to_index, tolerance))
        df_function['nearest_index'] = df_function['nearest_simTime'].map(sim_time_to_index)

        #确保df_function中的nearest_index为整数类型，且去掉NaN值
        df_function_renamed = df_function.rename(columns={'simTime': 'function_simTime'})#重命名 df_function 中的 simTime 列
        df_function_valid = df_function_renamed.dropna(subset=['nearest_index']).copy()
        df_function_valid['nearest_index'] = df_function_valid['nearest_index'].astype(int)

        ignore_cols = ['function_simTime', 'nearest_simTime', 'nearest_index']
        merged_df = mergecopy_by_simtime(merged_df, df_function_valid,ignore_cols)
        """
        def check_matching(df_function, sim_time_to_index, tolerance=0.01):
            #检查 function.csv 中的所有行是否都成功匹配
            
            # 计算每个 simTime 对应的 nearest_simTime
            df_function['nearest_simTime'] = df_function['simTime'].apply(lambda x: find_closest_time(x, sim_time_to_index, tolerance))
            
            # 检查是否有没有匹配到的行
            unmatched_rows = df_function[df_function['nearest_simTime'].isna()]
            if not unmatched_rows.empty:
                print(f"没有匹配上的行: {unmatched_rows}")
            else:
                print("所有行都成功匹配！")
            

            # 统计匹配上了的行数和没有匹配上的行数
            total_rows = len(df_function)
            matched_rows = len(df_function) - len(unmatched_rows)
            print(f"总行数: {total_rows}, 匹配上的行数: {matched_rows}, 没有匹配上的行数: {len(unmatched_rows)}")
            
            return unmatched_rows
        # 调用检查函数
        unmatched_rows = check_matching(df_function, sim_time_to_index, tolerance=0.01)
        # 获取最后一行的 simTime
        last_row_simtime = df_function.iloc[-1]['simTime']
        print(f"最后一行的 simTime: {last_row_simtime}")
        # 获取最后一行的 nearest_simTime
        last_row_nearest_simtime = df_function.iloc[-1]['nearest_simTime']
        print(f"最后一行的 nearest_simTime: {last_row_nearest_simtime}")
        """
        # 将弧度转换为角度
        merged_df['posH'] = merged_df['posH'].apply(convert_heading)
        if 'posP' in merged_df.columns:
            merged_df.rename(columns={'posP': 'pitch_rate'}, inplace=True)
        if 'posR' in merged_df.columns:
            merged_df.rename(columns={'posR': 'roll_rate'}, inplace=True)
            
        # 先使用 infer_objects 来确保类型一致
        merged_df = merged_df.infer_objects()
        merged_df.fillna(np.nan, inplace=True)  # 确保空值填充为 NaN
        merged_csv_path = Path(data_path) / "merged_ObjState.csv"

        # merged_df.to_csv(merged_csv_path, index=False,na_rep="NaN")
        merged_df.to_csv(merged_csv_path, index=False)
      
        return merged_csv_path


        
    # @ErrorHandler.measure_performance
    # def process_built_in_data(self) -> Dict[str, Path]:
    #     """实现PGVIL特有的内置数据处理逻辑
        
    #     处理顺序：
    #     1. 处理CAN数据
    #     2. 处理传感器数据
    #     3. 处理其他PGVIL特有数据
    #     4. 合并内置数据
        
    #     Returns:
    #         处理结果文件路径字典
    #     """
    #     result_files = {}
        
    #     # 1. 处理CAN数据
    #     print("1. 处理CAN数据...")
    #     can_results = self._process_can_data()
    #     if can_results:
    #         result_files.update(can_results)
    #     else:
    #         print("警告: CAN数据处理失败或无数据")
            
    #     # 2. 处理传感器数据
    #     print("\n2. 处理传感器数据...")
    #     sensor_results = self._process_sensor_data()
    #     if sensor_results:
    #         result_files.update(sensor_results)
    #     else:
    #         print("警告: 传感器数据处理失败或无数据")
            
    #     # 3. 处理其他PGVIL特有数据
    #     print("\n3. 处理其他PGVIL数据...")
    #     other_results = self._process_other_data()
    #     if other_results:
    #         result_files.update(other_results)
            
    #     # 4. 合并内置数据
    #     print("\n4. 合并内置数据...")
    #     if not self._merge_built_in_data(result_files):
    #         print("警告: 内置数据合并失败")
            
    #     return result_files
        
    # def _process_can_data(self) -> Dict[str, Path]:
    #     """处理CAN数据"""
    #     # TODO: 实现CAN数据处理逻辑
    #     return {}
        
    # def _process_sensor_data(self) -> Dict[str, Path]:
    #     """处理传感器数据"""
    #     # TODO: 实现传感器数据处理逻辑
    #     return {}
        
    # def _process_other_data(self) -> Dict[str, Path]:
    #     """处理其他PGVIL特有数据"""
    #     # TODO: 实现其他数据处理逻辑
    #     return {}
        
    # def _merge_built_in_data(self, result_files: Dict[str, Path]) -> bool:
    #     """合并PGVIL内置数据
        
    #     Args:
    #         result_files: 处理结果文件路径字典
            
    #     Returns:
    #         合并是否成功
    #     """
    #     try:
    #         # 实现PGVIL特有的数据合并逻辑
    #         return True
    #     except Exception as e:
    #         print(f"内置数据合并失败: {e}")
    #         return False