-
Notifications
You must be signed in to change notification settings - Fork 0
/
batch_cl_test.py
184 lines (164 loc) · 8.47 KB
/
batch_cl_test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
import torch
import argparse
import os
import sys
import logging
import pandas as pd
import tensorflow as tf
from statistic import static_result
from utils.riskmap.rm_utils import load_cfg_here
from utils.simulator import *
from utils.test_utils import *
from common_utils import *
from waymo_open_dataset.protos import scenario_pb2
import ray
import math
import glob
from utils.train_utils import set_seed
os.environ["DIPP_ABS_PATH"] = os.path.dirname(os.path.abspath(__file__))
sys.path.append(os.getenv('DIPP_ABS_PATH'))
@ray.remote
def closed_loop_test(test_pkg_sid=0, test_pkg_eid=100, pid=0):
# logging
log_path = f"./testing_log/{args.name}/"
os.makedirs(log_path, exist_ok=True)
os.makedirs(os.path.join(log_path,'video'), exist_ok=True)
initLogging(log_file=log_path+'test.log')
logging.info("------------- {} -------------".format(args.name))
logging.info("Use integrated planning module: {}".format(args.use_planning))
logging.info("Use device: {}".format(args.device))
# test file
# set up simulator
simulator = Simulator(150) # temporal horizon 15s
# load model
predictor, start_epoch, _ = load_checkpoint(args.model_path, map_location='cpu')
predictor.to(args.device)
logging.info(f'ckpt successful loaded from {args.model_path}')
predictor.eval()
# cache results
collisions, off_routes, traffic_light, progress = [], [], [], []
Accs, Jerks, Lat_Accs = [], [], []
Human_Accs, Human_Jerks, Human_Lat_Accs = [], [], []
similarity_3s, similarity_5s, similarity_10s = [], [], []
# set up planner
planner = init_planner(args, cfg, predictor)
for i,file in enumerate(files[test_pkg_sid: test_pkg_eid]):
scenarios = tf.data.TFRecordDataset(file)
logging.info(f'\n >>>[{i}/{len(files)}]:')
# iterate scenarios in the test file
length = [1 for s in scenarios]
for ii, scenario in enumerate(scenarios):
if ii>=args.scenario_num:
break
logging.info(f'\n >>>scenario {ii}/{len(length)}')
parsed_data = scenario_pb2.Scenario()
parsed_data.ParseFromString(scenario.numpy())
simulator.load_scenario(parsed_data)
logging.info(f'Scenario: {simulator.scenario_id}')
obs = simulator.reset()
done = False
TL = False
if type(obs) == type(None):
continue
while not done:
logging.info(f'Time: {simulator.timestep-19}')
batch = []
for i in range(len(obs)):
batch.append(torch.from_numpy(obs[i]))
if not args.gt_replay:
plan_traj,prediction = inference(batch, predictor, planner, args, args.use_planning, parallel=args.smoothing)
plan_traj = plan_traj.cpu().detach().numpy()[0]
prediction = prediction.cpu().detach().numpy()[0]
else:
g_traj = simulator.sdc_route[simulator.timestep+1:simulator.timestep+51]
current_pose = simulator.sdc_route[simulator.timestep]
plan_traj = g_traj.copy()
x = (g_traj[...,0]-current_pose[0])
y = (g_traj[...,1]-current_pose[1])
theta = -current_pose[2]
plan_traj[...,0] = x*math.cos(theta) - y*math.sin(theta)
plan_traj[...,1] = x*math.sin(theta) + y*math.cos(theta)
plan_traj[...,2] = g_traj[...,2] - current_pose[2]
# plan_traj = simulator.sdc_route[simulator.timestep+1:simulator.timestep+51]
prediction = np.zeros([10,50,3])#simulator.sdc_prediction
# take one step
obs, done, info = simulator.step(plan_traj, prediction)
TL = info[2] if TL==False else TL
logging.info(f'Collision: {info[0]}, Off-route: {info[1]}, traffic_light: {TL}')
# render
if args.render:
simulator.render(predictor=predictor, planner=planner, show_vf_map=args.show_vf_map)
# calculate metrics
collisions.append(info[0])
off_routes.append(info[1])
traffic_light.append(TL)
progress.append(simulator.calculate_progress())
dynamics = simulator.calculate_dynamics()
acc = np.mean(np.abs(dynamics[0]))
jerk = np.mean(np.abs(dynamics[1]))
lat_acc = np.mean(np.abs(dynamics[2]))
Accs.append(acc)
Jerks.append(jerk)
Lat_Accs.append(lat_acc)
error, human_dynamics = simulator.calculate_human_likeness()
h_acc = np.mean(np.abs(human_dynamics[0]))
h_jerk = np.mean(np.abs(human_dynamics[1]))
h_lat_acc = np.mean(np.abs(human_dynamics[2]))
Human_Accs.append(h_acc)
Human_Jerks.append(h_jerk)
Human_Lat_Accs.append(h_lat_acc)
similarity_3s.append(error[29])
similarity_5s.append(error[49])
similarity_10s.append(error[99])
# save animation
if args.save:
simulator.save_animation(os.path.join(log_path,'video/'))
# save metircs
data={'collision':collisions, 'off_route':off_routes, 'traffic_light':traffic_light,
'progress': progress,
'Acc':Accs, 'Jerk':Jerks, 'Lat_Acc':Lat_Accs,
'Human_Acc':Human_Accs, 'Human_Jerk':Human_Jerks, 'Human_Lat_Acc':Human_Lat_Accs,
'Human_L2_3s':similarity_3s, 'Human_L2_5s':similarity_5s, 'Human_L2_10s':similarity_10s}
df = pd.DataFrame(data=data)
df.to_csv(f"./testing_log/{args.name}/testing_log_cl_{pid}.csv")
# static_result(df, args.name, True)
return data
if __name__ == "__main__":
# Arguments
parser = argparse.ArgumentParser(description='Closed-loop Test')
parser.add_argument('--name', type=str, help='log name (default: "Test1")', default="Test1")
parser.add_argument('--config', type=str, help='path to the config file', default= None)
# parser.add_argument('--batch_size', type=int, help='test package counts', default=3)
parser.add_argument('--num_workers', type=int, help='test process counts', default=48)
parser.add_argument('--test_set', type=str, help='path to testing datasets')
parser.add_argument('--test_pkg_sid', type=int, help='start package counts', default=0)
parser.add_argument('--test_pkg_num', type=int, help='test package counts', default=3)
parser.add_argument('--scenario_num', type=int, help='scenario counts per file', default=1000)
parser.add_argument('--model_path', type=str, help='path to saved model')
parser.add_argument('--use_planning', action="store_true", help='if use integrated planning module (default: False)', default=False)
parser.add_argument('--render', action="store_true", help='if render the scene (default: False)', default=False)
parser.add_argument('--save', action="store_true", help='if save animation (default: False)', default=False)
parser.add_argument('--device', type=str, help='run on which device (default: cuda)', default='cuda')
parser.add_argument('--smoothing', type=str, help='enable after smoothingm include(default: none), single, mt, mp', default='none')
parser.add_argument('--gt_replay', action="store_true", help='if replay ground truth (default: False)', default=False)
parser.add_argument('--opt_plan', action="store_true", help='enable optimization based planning', default=False)
parser.add_argument('--rand_seed', type=int, help='optimization based planning steps', default=3407)
parser.add_argument('--show_vf_map', action="store_true", help='show cost/velocity map in render process', default=False)
args = parser.parse_args()
cfg_file = args.config if args.config else 'config.yaml'
os.environ["DIPP_CONFIG"] = str(os.getenv('DIPP_ABS_PATH') + '/' + cfg_file)
cfg = load_cfg_here()
set_seed(args.rand_seed)
# Run
files = glob.glob(args.test_set+'/*')[args.test_pkg_sid:args.test_pkg_sid+args.test_pkg_num]
num_files = len(files)
num_cpus = args.num_workers
workload = math.ceil(num_files/num_cpus)
ray.init(num_cpus=num_cpus)
ids = [closed_loop_test.remote(i*workload, min((i+1)*workload, num_files), i) for i in range(num_cpus)]
logging.info('process ids', ids)
results = ray.get(ids)
all_data = cellect_results(results)
all_df = pd.DataFrame(data=all_data)
static_result(all_df, args.name, True)
ray.shutdown()