def main(): target_waypoints_bak = [] # Initialize pygame pygame.init() # Initialize actors, display configuration and clock actor_list = [] display = pygame.display.set_mode((800, 600), pygame.HWSURFACE | pygame.DOUBLEBUF) font = get_font() clock = pygame.time.Clock() # Build connection between server and client client = carla.Client('localhost', 2000) client.set_timeout(2.0) # Get the world class world = client.get_world() # actor_list = world.get_actors() # can get actors like traffic lights, stop signs, and spectator #carla.Transform(carla.Location(x=64, y = 70, z=15), carla.Rotation(pitch=-80, yaw = 0))) # set a clear weather # weather = carla.WeatherParameters(cloudyness=0.0, precipitation=0.0, sun_altitude_angle=90.0) # world.set_weather(weather) world.set_weather(carla.WeatherParameters.ClearNoon) # Some global variables global should_auto should_auto = False global should_save should_save = False global should_slip should_slip = False global should_quit should_quit = False try: m = world.get_map() spawn_points = m.get_spawn_points() print("total number of spawn_points", len(spawn_points)) # Initialize spawn configuration spawn_config = 1 start_pose, destination = choose_spawn_destination( m, spawn_config=spawn_config) # Manually choose spawn location #start_pose = carla.Transform(location=carla.Location(x=-6.446170, y=-79.055023)) print("start_pose") print(start_pose) # Manually choose destination destination = carla.Transform( location=carla.Location(x=121.678581, y=61.944809, z=-0.010011)) #?????destination = carla.Transform(location=carla.Location(x=-2.419357, y=204.005676, z=1.843104)) print("destination") print(destination) # Find the first waypoint equal to spawn location print("Start waypoint", start_pose.location) start_waypoint = m.get_waypoint(start_pose.location) print("End waypoint", destination.location) end_waypoint = m.get_waypoint(destination.location) # Get blueprint library blueprint_library = world.get_blueprint_library() # set a constant vehicle vehicle_temp = random.choice( blueprint_library.filter('vehicle.lincoln.mkz2017')) vehicle = world.spawn_actor(vehicle_temp, start_pose) actor_list.append(vehicle) vehicle.set_simulate_physics(True) # Set a on-vehicle rgb camera """ camera_rgb = world.spawn_actor( blueprint_library.find('sensor.camera.rgb'), carla.Transform(carla.Location(x=-5.5, z=5.8), carla.Rotation(pitch=-35)), attach_to=vehicle) """ camera_rgb = world.spawn_actor( blueprint_library.find('sensor.camera.rgb'), #starting position #carla.Transform(carla.Location(x=110, y =55, z=15), carla.Rotation(pitch=-45, yaw = 180))) #friction # Angle #carla.Transform(carla.Location(x=58, y = 72, z=15), carla.Rotation(pitch=-45, yaw = -40))) # bird's view #carla.Transform(carla.Location(x=64, y = 70, z=15), carla.Rotation(pitch=-80, yaw = 0))) # closer carla.Transform(carla.Location(x=73, y=73, z=15), carla.Rotation(pitch=-90, yaw=0))) actor_list.append(camera_rgb) # Set a on-vehicle perceptron module camera_semseg = world.spawn_actor( blueprint_library.find('sensor.camera.semantic_segmentation'), carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), attach_to=vehicle) actor_list.append(camera_semseg) # Create a basic agent in the vehicle target_speed = 30 vehicle_agent = BasicAgent(vehicle, target_speed=target_speed) destination_loc = destination.location vehicle_agent.set_destination( (destination_loc.x, destination_loc.y, destination_loc.z)) # Initialize an NN controller from file nn_number_waypoints = 5 model = mlp(nx=(5 + 3 * nn_number_waypoints), ny=2) checkpoint = torch.load(model_path) model.load_state_dict(checkpoint['model_state_dict']) model = model.to(dtype).to(device) model.eval() print("Model loaded") # Initialize an ILQR controller controller = ILQRController(target_speed, steps_ahead=100, dt=0.05, l=1.0, half_width=1.5) ilqr_number_waypoints = controller.steps_ahead """ Collect data to file""" csv_dir = build_file_base( epoch, timestr, spawn_config, nn_number_waypoints, target_speed, info='friction_safe_150cm_run') if save_data else None # Create a synchronous mode context. MIN_DISTANCE_PERCENTAGE = 0.9 MIN_DISTANCE = target_speed / 3.6 * MIN_DISTANCE_PERCENTAGE nn_min_distance = MIN_DISTANCE nn_target_waypoints = deque(maxlen=nn_number_waypoints) ilqr_min_distance = 0.0 ilqr_target_waypoints = deque(maxlen=ilqr_number_waypoints) visual_number_waypoints = 1 visual_target_waypoints = deque(maxlen=visual_number_waypoints) max_distance = 1.5 #vehicle.set_autopilot(True) # Start simulation with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: nn_trajectory = [] ilqr_trajectory = [] tot_episodes = 0 tot_time = 0 top_spd = 0 avg_spd = 0 tot_unsafe_time = 0 tot_unsafe_episodes = 0 tot_interference_episodes = 0 tot_interference_time = 0 while True: get_event() # Quit the game once ESC is pressed if should_quit: return # Spawn Trigger Friction once f key is pressed avoidances = [carla.Location(64, 62, 0)] #avoidances = [carla.Location(83, 50, 0)] if should_slip: for avoidance in avoidances: friction_bp = world.get_blueprint_library().find( 'static.trigger.friction') friction_bp, friction_transform, friction_box = config_friction(friction_bp, \ location = avoidance, \ extent = carla.Location(6., 2., 0.2),\ color = (255, 127, 0)) world.debug.draw_box(**friction_box) frictioner = world.spawn_actor(friction_bp, friction_transform) actor_list.append(frictioner) should_slip = False clock.tick(30) # Get the current measurements t = vehicle.get_transform() v = vehicle.get_velocity() measurements_dict = {"v": v, "t": t} measurements = AttrDict(measurements_dict) print("Location:", measurements.t.location) spd = np.linalg.norm([v.x, v.y], ord=2) print("Velocity:", spd) if top_spd <= spd: top_spd = spd print("New top speed: {}".format(top_spd)) nn_min_distance = MIN_DISTANCE ilqr_min_distance = spd * controller.dt # get last waypoint current_waypoint = m.get_waypoint(t.location) if distance_vehicle(current_waypoint, t) > max_distance: tot_unsafe_episodes += 1 tot_unsafe_time += controller.dt """ (visualize) Query ground true future waypoints """ query_target_waypoints(current_waypoint, \ target_speed, visual_number_waypoints, visual_target_waypoints, \ m = m, measurements = measurements, min_distance = nn_min_distance * 0.1) for visual_target_waypoint in visual_target_waypoints: world.debug.draw_box(**config_waypoint_box( visual_target_waypoint, color=(0, 255, 0))) """ Collect true waypoints with PD controller """ # Run PD controllerr control_auto, target_waypoint = vehicle_agent.run_step( debug=False) # Draw PD controller target waypoints #world.debug.draw_box(**config_waypoint_box(target_waypoint, color = (0, 0, 0))) if should_save: # Start saving target waypoint to waypoint file print("Stored target waypoint {}".format(\ [target_waypoint.transform.location.x, target_waypoint.transform.location.y])) target_waypoints_bak.append([ target_waypoint.transform.location.x, target_waypoint.transform.location.y ]) # Run other controllers if not should_auto: # Draw nn controller waypoints #for horizon_waypoint in target_waypoints: # world.debug.draw_box(**config_waypoint_box(target_waypoint, color = (255, 0, 0))) # Run constant control # control = carla.VehicleControl(throttle=1, steer=0) """ Predict future nn trajectory and draw """ nn_trajectory_pred = get_nn_prediction(m, measurements, controller, model, \ ilqr_number_waypoints, target_speed, nn_number_waypoints) state = nn_trajectory_pred.states[0] # Draw predcted measurements #for i_measurements in nn_trajectory_pred.measurements: # world.debug.draw_box(**config_measurements_box(i_measurements,\ # color = (255, 0, 0))) us_init = np.array( nn_trajectory_pred.us[:ilqr_number_waypoints - 1]) print("Verify predicted nn trajectory") unsafe = verify_in_lane(nn_trajectory_pred, ilqr_number_waypoints, max_distance) #unsafe = verify_avoidance(nn_trajectory_pred, number_waypoints, avoidances, 1.0) if unsafe < ilqr_number_waypoints or no_interference: """ Get output from neural controller """ # Visualize world.debug.draw_box(**config_measurements_box( measurements, color=(255, 0, 0))) if unsafe: print( "!!!!!!!!!!!!!!\nUNSAFE NN operation number {}" .format(unsafe)) print("!!!!!!!!!!!!!!") # Query future waypoints query_target_waypoints(current_waypoint, \ target_speed, nn_number_waypoints, nn_target_waypoints, \ m = m, measurements = measurements, min_distance = nn_min_distance) # Draw future waypoints #for nn_target_waypoint in nn_target_waypoints: # world.debug.draw_box(**config_waypoint_box(nn_target_waypoint, color = (0, 255, 0))) # Build state for neural controller unnormalized_state, state = build_nn_state(measurements, \ target_speed, nn_number_waypoints, nn_target_waypoints) # Get nn control control_nn = get_nn_controller(state, model, device) control = control_nn nn_trajectory.append([ tot_episodes, control.throttle, control.steer, spd ]) else: print("!!!!!!!!!!!!!!\nUNSAFE NN operation number {}". format(unsafe)) print("!!!!!!!!!!!!!!") """ Run ilqr controller """ # Visualize world.debug.draw_box(**config_measurements_box( measurements, color=(0, 0, 255))) us_init = 0.0 * us_init # Query future waypoints query_target_waypoints(current_waypoint, \ target_speed, ilqr_number_waypoints, ilqr_target_waypoints, \ m = m, measurements = measurements, min_distance = ilqr_min_distance) # Draw future waypoints #for ilqr_target_waypoint in ilqr_target_waypoints: # world.debug.draw_box(**config_waypoint_box(ilqr_target_waypoint, color = (0, 0, 255))) # Get ilqr control control_ilqr, ilqr_trajectory_pred = get_ilqr_control(measurements, \ controller, ilqr_target_waypoints, avoidances = avoidances, us_init = us_init) control = control_ilqr print("Verify predicted ilqr trajectory") unsafe = verify_in_lane(ilqr_trajectory_pred, ilqr_number_waypoints, max_distance) #unsafe = verify_avoidance(ilqr_trajectory_pred, number_waypoints, avoidances, 2.0) if unsafe: print( "!!!!!!!!!!!!!!!!!!!!!!!!!\nUNSAFE ilqr operation number {}" .format(unsafe)) print("!!!!!!!!!!!!!!!!!!!!!!!!!") tot_interference_episodes += 1 tot_interference_time += controller.dt ilqr_trajectory.append([ tot_episodes, control.throttle, control.steer, spd ]) else: print("Auto pilot takes it from here") print("output from PD_control", control_auto.throttle, control_auto.steer) control = control_auto # Apply control to the actuator vehicle.apply_control(control) print("[Apply control]", control.throttle, control.steer) # Store vehicle information if leave_start_position(start_pose.location, vehicle) >= 2 or tot_episodes >= 10: # If having left the start position tot_episodes += 1 tot_time += clock.get_time() / 1e3 #controller.dt = clock.get_time()/1e3 #controller.steps_ahead = int(5./controller.dt) #ilqr_number_waypoints = controller.steps_ahead #print("Update ILQR steps ahead {}, step length {}".format(controller.steps_ahead,\ # controller.dt)) avg_spd += spd #nn_trajectory.append([control.throttle, control.steer, spd]) # Collect state-control training data # world.debug.draw_box(**config_measurements_box(i_measurements,\ # color = (255, 0, 0))) if csv_dir is not None: y_x = np.hstack( (np.array([control.throttle, control.steer]), state)) with open(csv_dir, 'a+') as csvFile: writer = csv.writer(csvFile) writer.writerow(y_x) # If reaching destination if reach_destination(destination_loc, vehicle) < 2.0 or ( control_auto.throttle == 0 and control_auto.steer == 0.0): if tot_episodes >= 10: raise Exception("Endgame") print("It is not the end!!!??") return print(">>>>Episode: {}".format(tot_episodes)) # Advance the simulation and wait for the data. snapshot, image_rgb, image_semseg = sync_mode.tick( timeout=2.0, vehicle=vehicle, control=control) image_semseg.convert(carla.ColorConverter.CityScapesPalette) fps = round(1.0 / snapshot.timestamp.delta_seconds) # Draw the display. draw_image(display, image_rgb) #draw_image(display, image_semseg, blend=True) display.blit( font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), (8, 10)) display.blit( font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), (8, 28)) pygame.display.flip() # raise ValueError("stop here") print('destroying local actors.') for actor in actor_list: actor.destroy() except Exception as e: print(e) #if tot_episodes <= 10: # os.remove(csv_dir) traceback.print_exc() finally: """ Store waypoints to file if len(target_waypoints_bak) is not 0: print("Store {} waypoints".format(len(target_waypoints_bak))) pickle.dump(target_waypoints_bak, open('./wps_at_plant_rotary_02.pt', 'wb')) else: print("Did not store target waypoints (length={})".format(len(target_waypoints_bak))) """ if should_quit: os.remove(csv_dir) print('destroying actors.') for actor in actor_list: actor.destroy() avg_spd /= tot_episodes print(">>>>>>>>>>> Total episodes: {} episodes||{}s <<<<<<<<<<<<<<". format(tot_episodes, tot_time)) print(">>>>>>>>>>>>>Total unsafe episodes: {}||{}s<<<<<<<<<<<<<<<<".format(tot_unsafe_episodes,\ tot_unsafe_time)) print(">>>>>>>>>>>>>>Total interference episodes: {}||{}s<<<<<<<<<".format(tot_interference_episodes,\ tot_unsafe_time)) print(">>>>>>>>>>>>>>Top speed: {} <<<<<<<<<<<<<<<<<<<<<<".format( top_spd)) print(">>>>>>>>>>>>>>Average speed: {}<<<<<<<<<<<<<<<<<<<".format( avg_spd)) pygame.quit() print('done.')
import numpy as np import matplotlib import matplotlib.pyplot as plt import torch import torch.nn.functional as F from torch.autograd import Variable # from torchviz import make_dot from torchsummary import summary from NN_controller import mlp torch.set_default_dtype(torch.float32) num_wps = 10 model = mlp(nx=(5 + 3 * num_wps), ny=2) sample_state = np.array([0.00414619210919023, -0.15624896240234376, -0.19012335205078126, -0.5009140014648438, 0.75, -0.15629389953613282, -0.20678996276855469, 1.4991319444444444, -0.15633935546875, -0.22345657348632814, \ 1.4991319444444444, -0.15638481140136717, -0.24012318420410156, 1.4991319444444444, -0.15643026733398438, -0.25678976440429685, 1.4991319444444444, -0.15645565795898436, -0.2734342346191406, 1.5011489868164063, \ -0.15625730895996093, -0.29004119873046874, 1.5064541286892361, -0.1557822265625, -0.3066424865722656, 1.511759270562066, -0.15503054809570313, -0.323233642578125, \ 1.5170644124348958, -0.1540864562988281, -0.33986141967773437, 1.5181676228841146, -0.152741455078125, -0.356009033203125, -0.44086286756727433]).astype(np.float32).reshape(1, -1) print("sample_state", sample_state.shape) state = torch.from_numpy(sample_state) MLP_dict_path = "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_dict_nx=8_wps10_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth" device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") checkpoint = torch.load(MLP_dict_path) model.load_state_dict(checkpoint['model_state_dict']) model = model.to(device)
def main(): target_waypoints_bak = [] # Initialize pygame pygame.init() # Initialize actors, display configuration and clock actor_list = [] display = pygame.display.set_mode((800, 600), pygame.HWSURFACE | pygame.DOUBLEBUF) font = get_font() clock = pygame.time.Clock() # Build connection between server and client client = carla.Client('localhost', 2000) client.set_timeout(2.0) # Get the world class world = client.get_world() # actor_list = world.get_actors() # can get actors like traffic lights, stop signs, and spectator # set a clear weather # weather = carla.WeatherParameters(cloudyness=0.0, precipitation=0.0, sun_altitude_angle=90.0) # world.set_weather(weather) world.set_weather(carla.WeatherParameters.ClearNoon) # Some global variables global should_auto should_auto = False global should_save should_save = False global should_slip should_slip = False global should_quit should_quit = False try: m = world.get_map() spawn_points = m.get_spawn_points() print("total number of spawn_points", len(spawn_points)) # Initialize spawn configuration spawn_config = 1 start_pose, destination = choose_spawn_destination( m, spawn_config=spawn_config) # Manually choose spawn location #start_pose = carla.Transform(location=carla.Location(x=-6.446170, y=-79.055023)) print("start_pose") print(start_pose) # Manually choose destination destination = carla.Transform( location=carla.Location(x=121.678581, y=61.944809, z=-0.010011)) #?????destination = carla.Transform(location=carla.Location(x=-2.419357, y=204.005676, z=1.843104)) print("destination") print(destination) # Find the first waypoint equal to spawn location print("Start waypoint", start_pose.location) start_waypoint = m.get_waypoint(start_pose.location) print("End waypoint", destination.location) end_waypoint = m.get_waypoint(destination.location) # Get blueprint library blueprint_library = world.get_blueprint_library() # set a constant vehicle vehicle_temp = random.choice( blueprint_library.filter('vehicle.lincoln.mkz2017')) vehicle = world.spawn_actor(vehicle_temp, start_pose) actor_list.append(vehicle) vehicle.set_simulate_physics(True) # Set a on-vehicle rgb camera camera_rgb = world.spawn_actor( blueprint_library.find('sensor.camera.rgb'), carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), attach_to=vehicle) actor_list.append(camera_rgb) # Set a on-vehicle perceptron module camera_semseg = world.spawn_actor( blueprint_library.find('sensor.camera.semantic_segmentation'), carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), attach_to=vehicle) actor_list.append(camera_semseg) # Create a basic agent in the vehicle target_speed = 30 vehicle_agent = BasicAgent(vehicle, target_speed=target_speed) destination_loc = destination.location vehicle_agent.set_destination( (destination_loc.x, destination_loc.y, destination_loc.z)) # Initialize an NN controller from file number_waypoints = 5 model = mlp(nx=(5 + 3 * number_waypoints), ny=2) checkpoint = torch.load(model_path) model.load_state_dict(checkpoint['model_state_dict']) model = model.to(dtype).to(device) model.eval() print("Model loaded") # Initialize an ILQR controller controller = ILQRController(target_speed, steps_ahead=20) number_waypoints = controller.steps_ahead """ Collect data to file""" csv_dir = None #csv_dir = build_file_base(timestr, spawn_config, number_waypoints, target_speed) # Create a synchronous mode context. MIN_DISTANCE_PERCENTAGE = 0.9 min_distance = target_speed / 3.6 * MIN_DISTANCE_PERCENTAGE target_waypoints = deque(maxlen=number_waypoints) #vehicle.set_autopilot(True) # Start simulation with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: i_episode = 0 i_time = 0 top_spd = 0 avg_spd = 0 while True: get_event() # Quit the game once ESC is pressed if should_quit: return # Spawn Trigger Friction once f key is pressed if should_slip: friction_bp = world.get_blueprint_library().find( 'static.trigger.friction') friction_transform, friction_box = config_friction(friction_bp, \ location = carla.Location(62, 61, 0), \ extent = carla.Location(700., 700., 700.)) frictioner = world.spawn_actor(friction_bp, friction_transform) actor_list.append(frictioner) world.debug.draw_box(**friction_box) should_slip = False clock.tick(30) # Get the current measurements t = vehicle.get_transform() v = vehicle.get_velocity() measurements_dict = {"v": v, "t": t} measurements = AttrDict(measurements_dict) print("Location:", measurements.t.location) spd = np.linalg.norm([measurements.v.x, measurements.v.y], ord=2) print("Velocity:", spd) if top_spd <= spd: top_spd = spd print("New top speed: {}".format(top_spd)) # get last waypoint current_waypoint = m.get_waypoint(t.location) query_target_waypoints(current_waypoint, \ target_speed, number_waypoints, target_waypoints, \ measurements = measurements, min_distance = min_distance) # Draw future waypoints for target_waypoint in target_waypoints: world.debug.draw_box(**config_waypoint_box( target_waypoint, color=(0, 255, 0))) # Run PD controllerr control_auto, target_waypoint = vehicle_agent.run_step( debug=False) # Draw PD controller target waypoints world.debug.draw_box( **config_waypoint_box(target_waypoint, color=(0, 0, 255))) if should_save: # Start saving target waypoint to waypoint file print("Stored target waypoint {}".format(\ [target_waypoint.transform.location.x, target_waypoint.transform.location.y])) target_waypoints_bak.append([ target_waypoint.transform.location.x, target_waypoint.transform.location.y ]) # Run other controllers if not should_auto: """ Run ilqr controller """ control_ilqr = get_ilqr_control(measurements, controller, target_waypoints) print("output from ilqr_control", control_ilqr.throttle, control_ilqr.steer) control = control_ilqr # Draw nn controller waypoints #for horizon_waypoint in target_waypoints: # world.debug.draw_box(**config_waypoint_box(target_waypoint, color = (255, 0, 0))) # Run constant control # control = carla.VehicleControl(throttle=1, steer=0) """ Get output from neural controller # Build state for neural controller unnormalized_state, state = build_nn_state(measurements, \ target_speed, number_waypoints, target_waypoints) # Get nn control control_nn = get_nn_controller(state, model, device) control = control_nn """ else: print("Auto pilot takes it from here") print("output from PD_control", control_auto.throttle, control_auto.steer) control = control_auto # Apply control to the actuator vehicle.apply_control(control) # Store vehicle information if leave_start_position(start_pose.location, vehicle) >= 2 or i_episode >= 10: # If having left the start position i_episode += 1 i_time += clock.get_time() print("Episode: {}".format(i_episode)) avg_spd += spd # Collect state-PDcontrol training data y_x = np.hstack( (np.array([control.throttle, control.steer]), state)) if csv_dir is not None: with open(csv_dir, 'a+') as csvFile: writer = csv.writer(csvFile) writer.writerow(y_x) # If reaching destination if reach_destination(destination_loc, vehicle) < 2.0 or ( control_auto.throttle == 0 and control_auto.steer == 0.0): if i_episode >= 10: raise Exception("Endgame") print("It is not the end!!!??") return # Advance the simulation and wait for the data. snapshot, image_rgb, image_semseg = sync_mode.tick( timeout=2.0, vehicle=vehicle, control=control) image_semseg.convert(carla.ColorConverter.CityScapesPalette) fps = round(1.0 / snapshot.timestamp.delta_seconds) # Draw the display. draw_image(display, image_rgb) #draw_image(display, image_semseg, blend=True) display.blit( font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), (8, 10)) display.blit( font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), (8, 28)) pygame.display.flip() # raise ValueError("stop here") print('destroying local actors.') for actor in actor_list: actor.destroy() except Exception as e: print(e) #if i_episode <= 10: # os.remove(csv_dir) traceback.print_exc() finally: """ Store waypoints to file if len(target_waypoints_bak) is not 0: print("Store {} waypoints".format(len(target_waypoints_bak))) pickle.dump(target_waypoints_bak, open('./wps_at_plant_rotary_02.pt', 'wb')) else: print("Did not store target waypoints (length={})".format(len(target_waypoints_bak))) """ print('destroying actors.') for actor in actor_list: actor.destroy() avg_spd /= i_episode print(">>>>>>>>>>> Total Time: {} episodes||{}s <<<<<<<<<<<<<<".format( i_episode, i_time / 1e3)) print(">>>>>>>>>>>>>>Top speed: {} <<<<<<<<<<<<<<<<<<<<<<".format( top_spd)) print(">>>>>>>>>>>>>>Average speed: {}<<<<<<<<<<<<<<<<<<<".format( avg_spd)) pygame.quit() print('done.')
def main(): pygame.init() # init font for text message pygame.font.init() # myfont = pygame.font.SysFont('Comic Sans MS', 30) # see below hud_dim = [800, 600] # default: 800, 600 # collect data: 200, 88 display = pygame.display.set_mode((hud_dim[0], hud_dim[1]), pygame.HWSURFACE | pygame.DOUBLEBUF) font = get_font() clock = pygame.time.Clock() client = carla.Client('localhost', 2000) client.set_timeout(2.0) world = client.get_world() world.set_weather(carla.WeatherParameters.ClearNoon) blueprint_library = world.get_blueprint_library() # actor_list = world.get_actors() # can get actors like traffic lights, stop signs, and spectator global_actor_list = [] save_dir_base = 'data/testing/' #MLP_model_path = "/home/depend/workspace/carla/PythonAPI/synchronous_mode/models/mlp/dest_start_SR0.5_models/mlp_model_nx=8_wps10_lr0.001_bs32_optimSGD_ep1000.pth" MLP_model_path = "/home/depend/workspace/carla/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_dict_nx=8_wps5_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth" # "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_model_nx=8_wps10_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth" # "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_model_nx=8_wps5_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth" # "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_model_nx=8_wps10_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth" # "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/mlp_img_model_nx=8_wps10_lr0.001_bs64_optimAdam_ep1000.pth" model = torch.load(MLP_model_path) checkpoint = torch.load(MLP_model_path) model = mlp(nx=5 + 3 * 5, ny=2) model.load_state_dict(checkpoint['model_state_dict']) model.eval() device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") try: m = world.get_map() spawn_points = m.get_spawn_points() print("total number of spawn_points", len(spawn_points)) # TODO: randomly choose spawn_point and destiny to test performance on new routes i = random.randint(0, len(spawn_points)) j = random.randint(0, len(spawn_points)) while j == i: j = random.randint(0, len(spawn_points)) i = 3 j = 0 # 2 save_dir = os.path.join(save_dir_base, 'data_{}_{}/'.format(i, j)) destiny = spawn_points[i] print(j, "car destiny", destiny.location) start_pose = spawn_points[j] print(i, "car start_pose", start_pose.location) # TODO: consider create a class of agent and use planner as in my_local_planner # waypoints_queue = deque(maxlen=20000) # use record and replay API # check /home/ruihan/.config/Epic/CarlaUE4/Saved path # client.start_recorder("record_testing_dest_{}_start_{}_wps10_1000.log".format(i, j)) # set and spawn actors actor_list = [] vehicle_bp = random.choice( blueprint_library.filter('vehicle.lincoln.mkz2017')) vehicle = world.spawn_actor(vehicle_bp, start_pose) vehicle.set_simulate_physics(True) actor_list.append(vehicle) # common camera locations: # x=1.6, z=1.7 for front # x=-5.5, z=2.8 for back camera_rgb_bp = blueprint_library.find('sensor.camera.rgb') camera_rgb_bp.set_attribute('image_size_x', str(hud_dim[0])) camera_rgb_bp.set_attribute('image_size_y', str(hud_dim[1])) camera_rgb = world.spawn_actor(camera_rgb_bp, carla.Transform( carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), attach_to=vehicle) actor_list.append(camera_rgb) camera_semseg_bp = blueprint_library.find( 'sensor.camera.semantic_segmentation') camera_semseg_bp.set_attribute('image_size_x', str(hud_dim[0])) camera_semseg_bp.set_attribute('image_size_y', str(hud_dim[1])) camera_semseg = world.spawn_actor(camera_semseg_bp, carla.Transform( carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), attach_to=vehicle) actor_list.append(camera_semseg) # set PD control to the vehicle, fine here as long as you don't use vehicle_agent.run_step target_speed = 30 vehicle_agent = BasicAgent(vehicle, target_speed=target_speed) destiny_loc = destiny.location vehicle_agent.set_destination( (destiny_loc.x, destiny_loc.y, destiny_loc.z)) # vehicle.set_autopilot(True) print("local actor list", actor_list) img_width = 200 img_height = 88 num_wps = 5 MIN_DISTANCE_PERCENTAGE = 0.9 min_distance = target_speed / 3.6 * MIN_DISTANCE_PERCENTAGE target_waypoints = deque(maxlen=num_wps) safety_checking = False # Create a synchronous mode context. with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=20) as sync_mode: control = vehicle.get_control() while reach_destiny(destiny_loc, vehicle) > 10: if should_quit(): print('destroying local actors.') for actor in actor_list: actor.destroy() return clock.tick(20) t = vehicle.get_transform() v = vehicle.get_velocity() # Instead of using BasicAgent, query target_waypoint and feed in NN controller last_waypoint = m.get_waypoint(t.location) # Method 1. query the target-waypoints based on current location of each frame # # query target_waypoints based on current location # target_waypoints = [] # for k in range(num_wps): # target_waypoint, road_option = compute_target_waypoint(last_waypoint, target_speed) # target_waypoints.append([target_waypoint, road_option]) # last_waypoint = target_waypoint # target_waypoints_np = [] # for wp_ro in target_waypoints: # target_waypoints_np.append(transform_to_arr(wp_ro[0].transform)) # target_waypoints_np = np.array([target_waypoints_np]).flatten() # Method 2. Use target_waypoints buffer and pop out once reached # check if reach, pop out from the list (purge the queue of obsolete waypoints) print("before purge", len(target_waypoints)) max_index = -1 # print(list(enumerate(target_waypoints))) for num, waypoint in enumerate(target_waypoints): if distance_vehicle(waypoint, t) < min_distance: max_index = num if max_index >= 0: for num in range(max_index + 1): target_waypoints.popleft() print("after purge", len(target_waypoints)) for k in range(len(target_waypoints), num_wps): # append waypoint one step ahead target_waypoint, road_option = compute_target_waypoint( last_waypoint, target_speed) target_waypoints.append(target_waypoint) last_waypoint = target_waypoint # spawn a trolley to visualilze target_waypoint # actor_list.append(spawn_trolley(world, blueprint_library, x=target_waypoint.transform.location.x, y=target_waypoint.transform.location.y)) print("after refill", len(target_waypoints)) target_waypoints_np = [] for wp_ro in target_waypoints: target_waypoints_np.append( transform_to_arr(wp_ro.transform)) target_waypoints_np = np.array([target_waypoints_np]).flatten() cur_speed = np.linalg.norm(np.array([v.x, v.y])) # save the long state for data collection # state = np.hstack((cur_speed, transform_to_arr(t), target_speed, transform_to_arr(target_waypoint.transform))) full_state = np.hstack( (cur_speed, transform_to_arr(t), target_speed, target_waypoints_np)).flatten() # shape(188,) # print("full state") # print(full_state) # parse the state for NN testing unnormalized = np.hstack( (full_state[0:3], full_state[5], full_state[7])) state = np.hstack( (full_state[0] / max_speed_val, full_state[1] / max_pos_val, full_state[2] / max_pos_val, full_state[5] / max_yaw_val, full_state[7] / max_speed_val)) for j in range(num_wps): # concatenate x, y, yaw of future_wps unnormalized = np.hstack( (unnormalized, full_state[8 + j * 6], full_state[9 + j * 6], full_state[12 + j * 6])) state = np.hstack( (state, full_state[8 + j * 6] / max_pos_val, full_state[9 + j * 6] / max_pos_val, full_state[12 + j * 6] / max_yaw_val)) # actor_list.append(spawn_trolley(world, blueprint_library, x=full_state[8+j*6], y=full_state[9+j*6], z=10)) # print("unnormalized") # print(unnormalized) # # calculate the relative coordinates # state[1] = state[1] + spawn_points[0].location.x - start_pose.location.x # state[5] = state[5] + spawn_points[0].location.x - start_pose.location.x # state[2] = state[2] + spawn_points[0].location.y - start_pose.location.y # state[6] = state[6] + spawn_points[0].location.y - start_pose.location.y # Advance the simulation and wait for the data. snapshot, image_rgb, image_semseg = sync_mode.tick( timeout=5.0) # extend timeout=2.0 image_semseg.convert(carla.ColorConverter.CityScapesPalette) fps = round(1.0 / snapshot.timestamp.delta_seconds) frame_number = image_semseg.frame_number # save the data # image_semseg.save_to_disk(save_dir+'{:08d}'.format(frame_number)) if '_img_' in MLP_model_path: # process the img data as a tensor (without saving and reading in again), see _parse_image in manual_control.py print("process img online") array = np.frombuffer(image_semseg.raw_data, dtype=np.dtype("uint8")) array = np.reshape( array, (image_semseg.height, image_semseg.width, 4)) array = array[:, :, :3] array = array[:, :, ::-1] print(array.shape) control = get_nn_img_controller(array, state, model, device, img_height, img_width) else: control = get_nn_controller(state, model, device) pred_traj = [] if safety_checking: # check if the traj is safe # safe, unsafe_time, pred_traj = mpc_verify(world, blueprint_library, model, device, m, full_state, control, pred_traj) result = mpc_verify_2(world, blueprint_library, model, device, m, full_state, control, pred_traj) print("message") print(result.message) print("output") print(result.x) if result.success: print("NN control is safe", control.throttle, control.steer) else: print("NN is unsafe, use MPC instead") #TODO # control = compute_mpc_safe_control() # if not safe: # print("mpc predicts unsafe traj", unsafe_time) # textsurface = font.render('mpc predicts unsafe traj at {} step'.format(unsafe_time), False, (255, 255, 255)) # # output most conservative action # DISCOUNTED_RATIO = 1.5 # control.throttle, control.steer = control.throttle/DISCOUNTED_RATIO, control.steer/DISCOUNTED_RATIO # # raise ValueError("stop here") # else: # print("safe continue") # textsurface = font.render('Safe. Continue', False, (255, 255, 255)) print("control", control.throttle, control.steer) vehicle.apply_control(control) # save the data # path = save_dir+'{:08d}_ctv'.format(frame_number) # x = np.hstack((np.array([control.throttle, control.steer]), state)) # print("control and measurement", x) # save in two formats, one separate to work with img, one appended to csv file # np.save(path, x) # with open(csv_dir, 'a+') as csvFile: # writer = csv.writer(csvFile) # writer.writerow(x) # Draw the display. draw_image(display, image_rgb) draw_image(display, image_semseg, blend=True) # Render the text messafes display.blit( font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), (8, 10)) display.blit( font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), (8, 28)) display.blit( font.render( 'control: [{}, {}]'.format(control.throttle, control.steer), True, (255, 255, 255)), (8, 46)) if safety_checking: display.blit(textsurface, (8, 64)) pygame.display.flip() # wait() if len(pred_traj): # print("destroy pred_traj") for actor in pred_traj: actor.destroy() print('destroying local actors.') for actor in actor_list: actor.destroy() finally: print('destroying global actors.') for actor in global_actor_list: actor.destroy() pygame.quit() # record API # client.stop_recorder() print('done.')