|  |  | @ -19,7 +19,6 @@ from openpilot.tools.sim.lib.camerad import W, H | 
			
		
	
		
		
			
				
					
					|  |  |  | C3_POSITION = Vec3(0.0, 0, 1.22) |  |  |  | C3_POSITION = Vec3(0.0, 0, 1.22) | 
			
		
	
		
		
			
				
					
					|  |  |  | C3_HPR = Vec3(0, 0,0) |  |  |  | C3_HPR = Vec3(0, 0,0) | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  | VEHICLE_STARTING_POS = [110, 4.5] |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  | metadrive_simulation_state = namedtuple("metadrive_simulation_state", ["running", "done", "done_info"]) |  |  |  | metadrive_simulation_state = namedtuple("metadrive_simulation_state", ["running", "done", "done_info"]) | 
			
		
	
		
		
			
				
					
					|  |  |  | metadrive_vehicle_state = namedtuple("metadrive_vehicle_state", ["velocity", "position", "bearing", "steering_angle"]) |  |  |  | metadrive_vehicle_state = namedtuple("metadrive_vehicle_state", ["velocity", "position", "bearing", "steering_angle"]) | 
			
		
	
	
		
		
			
				
					|  |  | @ -51,7 +50,7 @@ def apply_metadrive_patches(arrive_dest_done=True): | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  | def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock, |  |  |  | def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock, | 
			
		
	
		
		
			
				
					
					|  |  |  |                       controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection, |  |  |  |                       controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection, | 
			
		
	
		
		
			
				
					
					|  |  |  |                       exit_event, op_engaged, test_duration, minimal_distance, test_run): |  |  |  |                       exit_event, op_engaged, test_duration, test_run): | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |   arrive_dest_done = config.pop("arrive_dest_done", True) |  |  |  |   arrive_dest_done = config.pop("arrive_dest_done", True) | 
			
		
	
		
		
			
				
					
					|  |  |  |   apply_metadrive_patches(arrive_dest_done) |  |  |  |   apply_metadrive_patches(arrive_dest_done) | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
	
		
		
			
				
					|  |  | @ -71,7 +70,6 @@ def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera | 
			
		
	
		
		
			
				
					
					|  |  |  |     env.reset() |  |  |  |     env.reset() | 
			
		
	
		
		
			
				
					
					|  |  |  |     env.vehicle.config["max_speed_km_h"] = 1000 |  |  |  |     env.vehicle.config["max_speed_km_h"] = 1000 | 
			
		
	
		
		
			
				
					
					|  |  |  |     lane_idx_prev, _ = get_current_lane_info(env.vehicle) |  |  |  |     lane_idx_prev, _ = get_current_lane_info(env.vehicle) | 
			
		
	
		
		
			
				
					
					|  |  |  |     env.vehicle.set_position(VEHICLE_STARTING_POS) |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |     simulation_state = metadrive_simulation_state( |  |  |  |     simulation_state = metadrive_simulation_state( | 
			
		
	
		
		
			
				
					
					|  |  |  |       running=True, |  |  |  |       running=True, | 
			
		
	
	
		
		
			
				
					|  |  | @ -100,9 +98,6 @@ def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera | 
			
		
	
		
		
			
				
					
					|  |  |  |   steer_ratio = 8 |  |  |  |   steer_ratio = 8 | 
			
		
	
		
		
			
				
					
					|  |  |  |   vc = [0,0] |  |  |  |   vc = [0,0] | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   total_distance = 0 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   prev_x_pos = VEHICLE_STARTING_POS[0] |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   while not exit_event.is_set(): |  |  |  |   while not exit_event.is_set(): | 
			
		
	
		
		
			
				
					
					|  |  |  |     vehicle_state = metadrive_vehicle_state( |  |  |  |     vehicle_state = metadrive_vehicle_state( | 
			
		
	
		
		
			
				
					
					|  |  |  |       velocity=vec3(x=float(env.vehicle.velocity[0]), y=float(env.vehicle.velocity[1]), z=0), |  |  |  |       velocity=vec3(x=float(env.vehicle.velocity[0]), y=float(env.vehicle.velocity[1]), z=0), | 
			
		
	
	
		
		
			
				
					|  |  | @ -112,9 +107,6 @@ def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera | 
			
		
	
		
		
			
				
					
					|  |  |  |     ) |  |  |  |     ) | 
			
		
	
		
		
			
				
					
					|  |  |  |     vehicle_state_send.send(vehicle_state) |  |  |  |     vehicle_state_send.send(vehicle_state) | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |     total_distance += abs(env.vehicle.position[0] - prev_x_pos) |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |     prev_x_pos = env.vehicle.position[0] |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |     if controls_recv.poll(0): |  |  |  |     if controls_recv.poll(0): | 
			
		
	
		
		
			
				
					
					|  |  |  |       while controls_recv.poll(0): |  |  |  |       while controls_recv.poll(0): | 
			
		
	
		
		
			
				
					
					|  |  |  |         steer_angle, gas, should_reset = controls_recv.recv() |  |  |  |         steer_angle, gas, should_reset = controls_recv.recv() | 
			
		
	
	
		
		
			
				
					|  |  | @ -145,10 +137,7 @@ def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera | 
			
		
	
		
		
			
				
					
					|  |  |  |         elif out_of_lane: |  |  |  |         elif out_of_lane: | 
			
		
	
		
		
			
				
					
					|  |  |  |           done_result = (True, {"out_of_lane" : True}) |  |  |  |           done_result = (True, {"out_of_lane" : True}) | 
			
		
	
		
		
			
				
					
					|  |  |  |         elif timeout: |  |  |  |         elif timeout: | 
			
		
	
		
		
			
				
					
					|  |  |  |           if total_distance < minimal_distance: |  |  |  |           done_result = (True, {"timeout" : True}) | 
			
				
				
			
		
	
		
		
			
				
					
					|  |  |  |             done_result = (True, {"minimal_distance" : True}) |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |           else: |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |             done_result = (True, {"timeout" : True}) |  |  |  |  | 
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |         simulation_state = metadrive_simulation_state( |  |  |  |         simulation_state = metadrive_simulation_state( | 
			
		
	
		
		
			
				
					
					|  |  |  |           running=False, |  |  |  |           running=False, | 
			
		
	
	
		
		
			
				
					|  |  | 
 |