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) # ############################### ####################################### #