markers = Markers.Markers(Markers.markers_clockwise, 4 * 107 / 100.)
Origin = int(2 * 1000 / 300. * 300 / 5)
Mult = 1000 / 300. * 50 / 5
#
#######################################
##############################
#
if 'N' not in locals():
print("Loading trajectory data . . .")
N = lo(trajectory_data_location)
if 'the_arenas' not in locals():
print("Creating arenas . . .")
arenas_tmp_lst = [
Potential_Fields.Direct_Arena_Potential_Field(Origin, Mult, markers),
Potential_Fields.Play_Arena_Potential_Field(Origin, Mult, markers),
Potential_Fields.Follow_Arena_Potential_Field(Origin, Mult, markers),
Potential_Fields.Furtive_Arena_Potential_Field(Origin, Mult, markers)
]
the_arenas = {}
for a in arenas_tmp_lst:
the_arenas[a['type']] = a
cars = {}
for car_name in ['Mr_Black', 'Mr_Silver', 'Mr_Yellow', 'Mr_Orange', 'Mr_Blue']:
cars[car_name] = Cars.Car(N, car_name, Origin, Mult, markers)
#
###############################
#######################################
if __name__ == "__main__":
DISPLAY_LEFT = True
if 'N' not in locals():
print("Loading trajectory data . . .")
N = lo(opjD('N_pruned.pkl'))
markers = Markers.Markers(Markers.markers_clockwise,4*107/100.)
Origin = int(2*1000/300.*300 / 5)
Mult = 1000/300.*50 / 5
the_arena = Potential_Fields.Play_Arena_Potential_Field(Origin,Mult,markers)
mode = the_arena['type']
the_arena['Image']['img'] = z2o(the_arena['Image']['img'])
if mode == 'Follow_Arena_Potential_Field':
the_arena['Image']['img'] *= 0.5
the_arena['Image']['img'] += 0.5
if 'INITALIZED' not in locals():
INITALIZED = True
cars = {}
for car_name in ['Mr_Black','Mr_Silver','Mr_Yellow','Mr_Orange','Mr_Blue']:
cars[car_name] = Cars.Car(N,car_name,Origin,Mult,markers)
run_name = 'direct_rewrite_test_28Apr17_17h23m15s_Mr_Black'
our_car = Cars.car_name_from_run_name(run_name)
T0 = cars[our_car]['runs'][run_name]['trajectory']['ts'][0]
def Car(N,car_name,origin,mult,markers):
D = {}
D['Purpose'] = d2s(inspect.stack()[0][3],':','Car object.')
D['car_name'] = car_name
D['potential_field'] = Potential_Fields.Arena_Potential_Field(origin,mult,markers)
D['runs'] = {}
D['n_for_heading'] = 15
for run_name in N[car_name].keys():
D['runs'][run_name] = {}
R = D['runs'][run_name]
R['trajectory'] = N[car_name][run_name]['self_trajectory']
R['list_of_other_car_trajectories'] = []
"""
for ot in N[car_name][run_name]['other_trajectories']:
other_run_name = ot['run_name']
other_car_name = car_name_from_run_name(other_run_name)
R['list_of_other_car_trajectories'].append( [other_car_name,other_run_name] )
"""
for other_run_name in N[car_name][run_name]['other_trajectories']:
other_car_name = car_name_from_run_name(other_run_name)
R['list_of_other_car_trajectories'].append( [other_car_name,other_run_name] )
def _rewind():
D['state_info'] = {}
#D['state_info']['positions'] = {}
D['state_info']['near_i'] = 0
D['state_info']['near_t'] = 0
D['state_info']['pts'] = []
D['state_info']['heading'] = None
D['rewind'] = _rewind
def _check_trajectory_point(traj,side,i,t):
assert(traj['ts'][i] <= t)
if traj['ts'][i] == t:
if traj[side]['t_vel'][i] > 2: # 1.788: # Above 4 mph
return False
if traj[side]['t_vel'][i]<0.2: #TEMP
return False
elif traj['camera_separation'][i] > 0.25: # almost larger than length of car
return False
elif traj[side]['timestamp_gap'][i] > 0.1: # missed data points
return False
elif length([traj[side]['x'][i],traj[side]['y'][i]]) > length(markers['xy'][0]):
return False
return True
assert(False)
def _valid_time_and_index(run_name,t):
traj = D['runs'][run_name]['trajectory']
if t>traj['ts'][0] and t<traj['ts'][-1]:
near_t = -1
for i in range(D['state_info']['near_i'],len(traj['ts'])):
if traj['ts'][i-1]<t and traj['ts'][i]>t:
near_t = traj['ts'][i]
near_i = i
break
if near_t > 0:
D['state_info']['near_i'] = near_i
D['state_info']['near_t'] = near_t
for side in ['left','right']:
if not _check_trajectory_point(traj,side,near_i,near_t):
return False,False
return near_t,near_i
return False,False
def _report_camera_positions(run_name,t):
near_t,near_i = _valid_time_and_index(run_name,t)
if not near_t:
return []
traj = D['runs'][run_name]['trajectory']
positions = []
for side in ['left','right']:
positions.append([traj[side]['x'][near_i],traj[side]['y'][near_i]])
D['state_info']['pts'].append(array(positions).mean(axis=0))
if len(D['state_info']['pts']) >= D['n_for_heading']:
n = D['n_for_heading']
D['state_info']['heading'] = normalized_vector_from_pts(D['state_info']['pts'][-n:])
if D['state_info']['pts'][-n][0] > D['state_info']['pts'][-1][0]:
D['state_info']['heading'] *= -1
else:
D['state_info']['heading'] = None
return D['state_info']['pts'][-1] #positions
D['report_camera_positions'] = _report_camera_positions
"""
def _report_camera_positions(run_name,t):
near_t,near_i = _valid_time_and_index(run_name,t)
if not near_t:
return False
traj = D['runs'][run_name]['trajectory']
positions = []
for side in ['left','right']:
positions.append([traj[side]['x'][near_i],traj[side]['y'][near_i]])
D['state_info']['pts'].append(positions[0])
if len(D['state_info']['pts']) >= D['n_for_heading']:
n = D['n_for_heading']
D['state_info']['heading'] = normalized_vector_from_pts(D['state_info']['pts'][-n:])
if D['state_info']['pts'][-n][0] > D['state_info']['pts'][-1][0]:
D['state_info']['heading'] *= -1
else:
D['state_info']['heading'] = None
return positions
D['report_camera_positions'] = _report_camera_positions
"""
def _get_left_image(run_name):
traj = D['runs'][run_name]['trajectory']
index = traj['data']['t_to_indx'][D['state_info']['near_t']]
img = traj['data']['left'][index]
return img
D['get_left_image'] = _get_left_image
def _load_image_and_meta_data(run_name,bair_car_data_location):
import data.utils.general
import data.utils.multi_preprocess_pkl_files_1
bag_folders_dst_rgb1to4_path = opj(bair_car_data_location,'rgb_1to4')
bag_folders_dst_meta_path = opj(bair_car_data_location,'meta')
D['runs'][run_name]['trajectory']['data'] = data.utils.general.get_new_Data_dic()
data.utils.multi_preprocess_pkl_files_1.multi_preprocess_pkl_files(
D['runs'][run_name]['trajectory']['data'],
opj(bag_folders_dst_meta_path,run_name),
opj(bag_folders_dst_rgb1to4_path,run_name),
print_b=True)
D['load_image_and_meta_data'] = _load_image_and_meta_data
return D
markers = Markers.Markers(Markers.markers_clockwise,4*107/100.)
Origin = int(2*1000/300.*300 / 5)
Mult = 1000/300.*50 / 5
#
#######################################
##############################
#
if 'N' not in locals():
print("Loading trajectory data . . .")
N = lo(trajectory_data_location)
if 'the_arenas' not in locals():
print("Creating arenas . . .")
warp_image = True
arenas_tmp_lst = [Potential_Fields.Direct_Arena_Potential_Field(Origin,Mult,markers,warp_image=warp_image),
Potential_Fields.Play_Arena_Potential_Field(Origin,Mult,markers,warp_image),
Potential_Fields.Follow_Arena_Potential_Field(Origin,Mult,markers,warp_image),
Potential_Fields.Furtive_Arena_Potential_Field(Origin,Mult,markers,warp_image)]
the_arenas = {}
for a in arenas_tmp_lst:
the_arenas[a['type']] = a
cars = {}
for car_name in ['Mr_Black','Mr_Silver','Mr_Yellow','Mr_Orange','Mr_Blue']:
cars[car_name] = Cars.Car(N,car_name,Origin,Mult,markers)
#
###############################
#######################################
#