import os
import sys
from datetime import datetime
import traceback
import json
import math
import pandas as pd
import subprocess
import time
import multiprocessing # 导入进程模块
from functools import partial # 导入偏函数
import warnings
warnings.filterwarnings("ignore")
from openx import Scenario, formatThree, formatTwo, change_CDATA
import osgbGenerator
from utils import smooth_1, get_coordinate_cow, get_map_name, reverse_gyb_gps
class Batchrun:
def __init__(self, path, keyFileName):
""""初始化方法"""
self.path = path
self.keyFileName = keyFileName
def getFile(self, path, keyFileName):
'''
获得可以进行轨迹提取的所有片段文件夹路径
Parameters
----------
path : TYPE
DESCRIPTION.
Returns
-------
FileList : TYPE
可以进行轨迹提取的所有片段文件夹路径.
'''
files = os.listdir(path) # 得到文件夹下的所有文件，包含文件夹名称
FileList = []
if keyFileName not in files:
for name in files:
if os.path.isdir(path +'/'+name):
FileList.extend(self.getFile(path +'/'+name, keyFileName)) #回调函数，对所有子文件夹进行搜索
else:
FileList.append(path)
FileList = list(set(FileList))
return FileList
def generateScenarios_raw(self, absPath, param):
'''
原始自然驾驶场景还原
'''
# 读取自车数据
posPath = os.path.join(absPath, 'pos.csv') # 用来生成场景中自车和他车的路径
posdata = pd.read_csv(posPath)
# 读取仿真地图数据
offset_x_gyb = -446403.548
offset_y_gyb = -4403799.286
offset_x_thq = -456256.260152
offset_y_thq = -4397809.886833
offset_x_glyt = -457000
offset_y_glyt = -4400000
#offset_h = 90 # 初始headinga 和 VTD 有 90偏角
offset_h = 0
thqPath = '/root/rosmerge/resource/thq_path.csv'
# thqPath = os.path.join(absPath, 'thq_path.csv')
thqdata = pd.read_csv(thqPath)
thqdata['posX'] -= offset_x_thq
thqdata['posY'] -= offset_y_thq
gybPath = '/root/rosmerge/resource/gybhl_path.csv'
# gybPath = os.path.join(absPath, 'gybhl_path.csv')
gybdata = pd.read_csv(gybPath)
gybdata['posX'] -= offset_x_gyb
gybdata['posY'] -= offset_y_gyb
reverse_east, reverse_north = reverse_gyb_gps(gybdata)
gybdata['East'] = reverse_east
gybdata['North'] = reverse_north
glytPath = '/root/rosmerge/resource/glyt_path.csv'
# glytPath = os.path.join(absPath, 'glyt_path.csv')
glytdata = pd.read_csv(glytPath)
glytdata['posX'] -= offset_x_glyt
glytdata['posY'] -= offset_y_glyt
init_x = posdata.iloc[0]['East']
init_y = posdata.iloc[0]['North']
map_id = get_map_name(init_x, init_y, thqdata, gybdata, glytdata)
# 对感知数据进行后处理
# 对公园北环路进行坐标转换
if map_id == 0:
offset_x = offset_x_thq
offset_y = offset_y_thq
elif map_id == 1:
new_east, new_north = get_coordinate_cow(posdata)
posdata['East'] = new_east
posdata['North'] = new_north
offset_x = offset_x_gyb
offset_y = offset_y_gyb
else:
offset_x = offset_x_glyt
offset_y = offset_y_glyt
# 根据pos文件规范时间戳
posdata['Time'] = posdata['Time'].apply(lambda x: (x // 100) * 100)
# 绝对高程与相对高程的差值
posdata['altitude'] = posdata['altitude'] - 9.1
pos_ego = posdata.loc[posdata['ID'] == -1, ['Time', 'East', 'North', 'HeadingAngle', 'altitude', 'Type']]
# pos_ego[['East', 'North']] = pos_ego.apply(get_coordinate_cow, axis=1)
# pos_ego = posdata.loc[posdata['ID'] == -1, ['Time', 'position_x', 'position_y', 'HeadingAngle', 'Type']]
pos_ego = pos_ego.reset_index(drop=True)
# offset_x = -float(pos_ego.at[0, 'East']) # 初始East 设为0
# offset_y = -float(pos_ego.at[0, 'North']) # 初始North 设为0
start_time = pos_ego.at[0, 'Time']
ego_points, gps_time, ego_type = Scenario.getXoscPosition(pos_ego, 'Time', 'East', 'North', 'HeadingAngle', 'altitude','Type', offset_x, offset_y, offset_h,start_time)
# ego_points, gps_time, ego_type = Scenario.getXoscPosition(pos_ego, 'Time', 'position_x', 'position_y', 'HeadingAngle', 'Type', offset_x, offset_y, offset_h, start_time)
# 读取目标数据
pos_obs = posdata[posdata['Type'] != 10] # 排除非目标物的物体
# pos_obs = posdata[posdata['Type'] != 10] # 排除非目标物的物体
pos_obs = pos_obs.loc[pos_obs['ID'] != -1, ['Time', 'ID', 'East', 'North', 'HeadingAngle', 'AbsVel', 'altitude', 'Type']]
# pos_obs = pos_obs.loc[pos_obs['ID'] != -1, ['Time', 'ID', 'position_x', 'position_y', 'HeadingAngle', 'Type']]
#!!!!自定义修改
# pos_obs.loc[pos_obs['ID'] == 1200, 'type'] = 7
# pos_obs.loc[pos_obs['ID'] == 1197, 'type'] = 7
# pos_obs.loc[pos_obs['ID'] == 1200, 'position_x'] += 1
# pos_obs.loc[pos_obs['ID'] == 1200, 'position_y'] -= 2
# pos_obs.loc[pos_obs['ID'] == 1197, 'position_x'] += 1
# pos_obs.loc[pos_obs['ID'] == 1197, 'position_y'] -= 1
# pos_obs.loc[pos_obs['ID'] == 1203, 'type'] = 4
# pos_obs.loc[pos_obs['ID'] == 1203, 'position_x'] = 456319
# pos_obs.loc[pos_obs['ID'] == 1203, 'position_y'] = 4397979
# pos_obs.loc[pos_obs['ID'] == 1203, 'HeadingAngle'] = 1
# offset_h = 30
# pos_obs.loc[pos_obs['ID'] == 1194, 'type'] = 7
# pos_obs.loc[pos_obs['ID'] == 1188, 'type'] = 2
# pos_obs.loc[pos_obs['ID'] == 1189, 'type'] = 2
# pos_obs.loc[pos_obs['ID'] == 1195, 'type'] = 2
# obsPath = os.path.join(absPath, 'obs.csv') # 用来筛选场景中不需要的他车
# obsdata = pd.read_csv(obsPath)
# obsdata = obsdata[(obsdata['ObjectPosY'] < 5) & (obsdata['ObjectPosY'] > -5) & (obsdata['ObjectPosX'] > -10) & (obsdata['ObjectPosX'] < 100)] # 排除车道线范围外且前向距离较远的目标物
# idlist = obsdata['ObjectID'].tolist() # 筛选出符合条件的的目标物ID
# pos_obs = pos_obs[(pos_obs['ID'].isin(idlist))]
pos_obs = pos_obs.reset_index(drop=True)
groups = pos_obs.groupby('ID')
object_points = []
for key, value in groups:
if len(value) < 5:
continue
# value = smooth_1(value)
object_points.append(Scenario.getXoscPosition(value, 'Time', 'East', 'North', 'HeadingAngle', 'altitude', 'Type', offset_x, offset_y, offset_h,start_time))
# object_points.append(Scenario.getXoscPosition(value, 'Time', 'position_x', 'position_y', 'HeadingAngle', 'type', offset_x, offset_y, offset_h,start_time))
ego_speed = 5
period = math.ceil(gps_time[-1] - gps_time[0])
work_mode = 0 # 0为CICV车端数据
hour = int(absPath.split('/')[-1].split('-')[3])
if hour + 8 >= 24:
hour = hour - 16
else:
hour = hour + 8
time_of_day = hour * 3600
# 没有路灯防止天过暗
if time_of_day >= 64800:
time_of_day = 64800
s = Scenario(ego_points, object_points, gps_time, ego_speed, work_mode, period, absPath, time_of_day, map_id)
filename = absPath + '/simulation'
files = s.generate(filename)
change_CDATA(files[0][0])
print(files)
# 生成osgb
# osgbGenerator.formatTwo(filename)
# 修改xosc路径
# formatThree(filename)
# # # 生成每个场景的描述文件 json
# # getLabel(output_path, scenario_series['场景编号'], scenario_series['场景名称'])
# 拷贝到vtd路径下
# vtd_path = os.path.join('/home/lxj/VIRES/VTD.2021.3/Data/Projects/Current/Scenarios/myScenario', datetime.now().strftime("%Y%m%d%H%M%S"))
# if not os.path.exists(vtd_path):
# os.makedirs(vtd_path)
# os.system('cp '+ files[0][0] + ' ' + vtd_path + '/')
def multiRun(self, path, param):
files = self.getFile(path, self.keyFileName)
print('程序开始，%s 个数据包' % len(files))
t1 = time.time()
# 无参数时，使用所有cpu核; 有参数时，使用CPU核数量为参数值
pool = multiprocessing.Pool(processes = 10)
pfunc = partial(self.generateScenarios_raw, param)
pool.map(pfunc, files)
# 调用join之前，先调用close函数，否则会出错。执行完close后不会有新的进程加入到pool,join函数等待所有子进程结束
pool.close()
pool.join()
t2 = time.time()
print ("程序结束，并行执行时间：%s s" % int(t2 - t1))
def batchRun(self, path, param):
files = self.getFile(path, self.keyFileName)
print('程序开始，%s 个数据包' % len(files))
for di, absPath in enumerate(sorted(files)):
print(absPath)
# try:
self.generateScenarios_raw(absPath, param)
# except:
# print('Augmentation failed!!!!')
# error = {'time':datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3],'traceback':traceback.format_exc()}
# with open('error.log','a+') as f:
# json.dump(error, f, indent=4)
# f.write('\n')
# print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
return None
def GenerateVideo(self, root_path):
# 批量生成视频
imagelist = self.getFile(root_path, 'image')
for item in imagelist:
strs = item.split('/')
respath = os.path.join(item,"video.mp4")
# respath = os.path.join(res_path,strs[-3],strs[-2],"video.mp4")
print('---------------------')
# os.makedirs(os.path.join(res_path,strs[-3],strs[-2]))
command = "ffmpeg -f image2 -r 10 -pattern_type glob -i '" + item + "/image/*.jpg" + "' -y '" + respath + "'"
print(command)
process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
process.wait()
# if __name__ == "__main__":
# rootPath = "/home/hancheng/test_1116/data_merge_2023-11-21-14-15-25_overspeed_12" # 跟车
#
# # 生成场景
# a = Batchrun(rootPath, "pos.csv")
# a.batchRun(rootPath, 0) # 0为占位参数
# # a.multiRun(rootPath, 0)
#
# # 生成视频
# # a.GenerateVideo(rootPath)
############################### mlx #########################################
if __name__ == "__main__":
rootPath = sys.argv[1] # 跟车
# 生成场景
a = Batchrun(rootPath, "pos.csv")
a.batchRun(rootPath, 0) # 0为占位参数
# a.multiRun(rootPath, 0)
# 生成视频
# a.GenerateVideo(rootPath)