-
Notifications
You must be signed in to change notification settings - Fork 0
/
raw_collector.py
167 lines (140 loc) · 6.83 KB
/
raw_collector.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
# This implements a raw data (no high-level detection) collectior car.
# The following boilerplate is required if .egg is not installed
# See: https://carla.readthedocs.io/en/latest/build_system/
import glob
import os
import datetime
import sys
try:
sys.path.append(glob.glob('./carla-*%d.%d-%s.egg' % (
sys.version_info.major,
sys.version_info.minor,
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
pass
import carla
import argparse
import yaml
import pickle
from math import pi
from shutil import copyfile
from carlasim.data_collect import World, IMU, GNSS, SemanticCamera, DepthCamera
from carlasim.record import SequentialRecorder, StaticAndSequentialRecorder
def main():
# Parse passed-in config yaml file
argparser = argparse.ArgumentParser(
description='CARLA Roaming Data Collector')
argparser.add_argument('config', type=argparse.FileType(
'r'), help='yaml file for carla simulation config')
argparser.add_argument('-r', '--record', default=False, action='store_true',
help='record data selected in config file')
args = argparser.parse_args()
# Read configurations from yaml file to config
with args.config as f:
config = yaml.safe_load(f)
# Placeholder for World obj
world = None
# Assign spawn point for ego vehicle according to config
if config['ego_veh']['use_random_spawn_point']:
spawn_point = None
else:
spawn_location = carla.Location(config['ego_veh']['spawn_location'][0],
config['ego_veh']['spawn_location'][1],
config['ego_veh']['spawn_location'][2])
spawn_orientation = carla.Rotation(config['ego_veh']['spawn_orientation'][0],
config['ego_veh']['spawn_orientation'][1],
config['ego_veh']['spawn_orientation'][2])
spawn_point = carla.Transform(spawn_location, spawn_orientation)
try:
client = carla.Client('localhost', 2000)
client.set_timeout(5.0)
# Create a World obj with a built-in map
world = World(client.load_world(config['world']['map']),
client.get_trafficmanager(),
config,
spawn_point=spawn_point)
# Add Carla sensors
ego_veh = world.ego_veh
world.add_carla_sensor(IMU('imu', config['sensor']['imu'], ego_veh))
world.add_carla_sensor(GNSS('gnss', config['sensor']['gnss'], ego_veh))
world.add_carla_sensor(SemanticCamera(
'semantic_camera', config['sensor']['front_camera'], ego_veh))
world.add_carla_sensor(DepthCamera(
'depth_camera', config['sensor']['front_camera'], ego_veh))
# Set up recorders
if args.record:
sensor_recorder = SequentialRecorder(config['recorder']['sensor'])
gt_recorder = StaticAndSequentialRecorder(config['recorder']['gt'])
# Record static ground truth data
gt_recorder.record_static(world.ground_truth.all_gt['static'])
# Launch autopilot for ego vehicle
world.set_ego_autopilot(True, config['autopilot'])
if args.record:
recording_folder = os.path.join(os.getcwd(), 'recordings',
datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S'))
os.makedirs(recording_folder)
# Set up Carla's recorder for replaying if activated by config
if config['carla_recorder']:
file_name = os.path.join(
recording_folder, 'carla_recording.log')
client.start_recorder(file_name)
n_ticks = int(config['sim_duration'] /
config['world']['delta_seconds'])
# Simulation loop
to_left = True
for idx in range(n_ticks):
world.step_forward()
world.see_ego_veh()
# Record sequential data
if args.record:
sensor_recorder.record_seq(world.all_sensor_data)
gt_recorder.record_seq(world.ground_truth.all_gt['seq'])
# Print data of interest
imu_data = world.all_sensor_data['imu']
lane_gt = world.ground_truth.all_gt['seq']['lane']
print('vx: {:3.2f}, vy: {:3.2f}, w: {:3.2f}'.format(
imu_data['vx'], imu_data['vy'], imu_data['gyro_z'] * 180 / pi))
print('in junction: {}'.format(
lane_gt['in_junction']))
# c0
print('{:.2f} {:.2f} {:.2f} {:.2f}'.format(
lane_gt['next_left_marking_coeffs'][0] if lane_gt['next_left_marking'] else -10,
lane_gt['left_marking_coeffs'][0] if lane_gt['left_marking'] else -10,
lane_gt['right_marking_coeffs'][0] if lane_gt['right_marking'] else -10,
lane_gt['next_right_marking_coeffs'][0] if lane_gt['next_right_marking'] else -10))
# c1
print('{:.2f} {:.2f} {:.2f} {:.2f}'.format(
lane_gt['next_left_marking_coeffs'][1] if lane_gt['next_left_marking'] else -10,
lane_gt['left_marking_coeffs'][1] if lane_gt['left_marking'] else -10,
lane_gt['right_marking_coeffs'][1] if lane_gt['right_marking'] else -10,
lane_gt['next_right_marking_coeffs'][1] if lane_gt['next_right_marking'] else -10))
# Marking type
print('{} {} {} {}'.format(
lane_gt['next_left_marking'].type if lane_gt['next_left_marking'] else None,
lane_gt['left_marking'].type if lane_gt['left_marking'] else None,
lane_gt['right_marking'].type if lane_gt['right_marking'] else None,
lane_gt['next_right_marking'].type if lane_gt['next_right_marking'] else None))
if (idx+1) % int(3/config['world']['delta_seconds']) == 0:
world.force_lane_change(to_left=to_left)
to_left = not to_left
if args.record:
# Stop Carla's recorder
if config['carla_recorder']:
client.stop_recorder()
# Save recorded data
sensor_recorder.save(recording_folder, 'sensor_data')
gt_recorder.save(recording_folder, 'gt_data')
# Copy config files to recording folder for future reference
settings_dir = os.path.join(recording_folder, 'settings')
os.mkdir(settings_dir)
target_dir = os.path.join(settings_dir, 'config.yaml')
copyfile(args.config.name, target_dir)
finally:
if world:
world.set_ego_autopilot(False)
world.destroy()
# Allow carla engine to run freely so it doesn't just hang there
world.allow_free_run()
# %%
if __name__ == '__main__':
main()