def run_step(self): transform = self._vehicle.get_transform() if self.waypoints: # If too far off course, reset waypoint queue. if distance_vehicle(self.waypoints[0], transform) > 5.0 * self.radius: self.waypoints = [] # Get more waypoints. if len(self.waypoints) < self.max_waypoints // 2: self.add_next_waypoints() # If no more waypoints, stop. if not self.waypoints: print('Ran out of waypoints; stopping.') control = carla.VehicleControl() control.throttle = 0.0 return control # Remove waypoints we've reached. while distance_vehicle(self.waypoints[0], transform) < self.min_dist: self.waypoints = self.waypoints[1:] # Draw next waypoint draw_waypoints(self._vehicle.get_world(), self.waypoints[:1], self._vehicle.get_location().z + 1.0) return self.controller.run_step(self.target_speed, self.waypoints[0])
def query_target_waypoints(current_waypoint, target_speed, number_waypoints, target_waypoints, road_option=None, **kwargs): # Method 1. query the target-waypoints based on current location of each frame # # query target_waypoints based on current location if 'measurements' not in kwargs.keys(): target_waypoints.clear() else: # Method 2. use the buffer target_waypoints to store the waypoints # Once a waypoint is reached pop one waypoint out and push a new waypoint in m = kwargs['m'] measurements = kwargs['measurements'] min_distance = kwargs['min_distance'] from_waypoint = m.get_waypoint(measurements.t.location) max_index = -1 for num, waypoint in enumerate(list(target_waypoints)): if distance_vehicle(waypoint, from_waypoint.transform) < min_distance: max_index = num if max_index >= 0: for num in range(1 + max_index): target_waypoints.popleft() if len(target_waypoints) > 0: from_waypoint = target_waypoints[-1] else: from_waypoint = current_waypoint for k in range(number_waypoints - len(target_waypoints)): target_waypoint, road_option = compute_target_waypoint( from_waypoint, target_speed) target_waypoints.append(target_waypoint) from_waypoint = target_waypoint
def run_step(self, debug=True): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ # not enough waypoints in the horizon? => add more! if not self._global_plan and len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): self._compute_next_waypoints(k=100) if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # target waypoint self.target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # move using PID controllers control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) <= self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) return control
def run_step(self): assert self._route is not None vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoints_queue): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): if self.distances: self.distance_to_goal -= self.distances[0] self.distances.popleft() self._waypoints_queue.popleft() if len(self._waypoints_queue) > 0: self.target = self._waypoints_queue[0]
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.')
def run_step(self, debug=True): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ # print("current queue len: ", len(self._waypoints_queue)) # print("maxlen: ", int(self._waypoints_queue.maxlen * 0.5)) # not enough waypoints in the horizon? => add more! # if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5): # print("no points here!") # if not self._global_plan: # self._compute_next_waypoints(k=100) # # 队列为空时切换到手动操作模式,待修改 # if len(self._waypoints_queue) == 0: #control = carla.VehicleControl() #control.steer = 0.0 #control.throttle = 0.0 #control.brake = 0.0 #control.hand_brake = False #control.manual_gear_shift = False # return control # Buffering the waypoints # print("queue length inside run_step: ", len(self._waypoints_queue)) if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: # print("queue length: ", len(self._waypoints_queue)) left_point, left_road = self._waypoints_queue.popleft() self._waypoint_buffer.append((left_point, left_road)) # right_point, _ = self._waypoint_buffer.popleft() # print("right point is ", right_point) else: break # target_waypoint, _ = self._waypoint_buffer[0] # print("target_waypoint:", target_waypoint) # for i, (waypoint, _) in enumerate(self._waypoint_buffer): # print("waypoint is ", waypoint) # time.sleep(2) # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) if not self._waypoint_buffer: # control = self._vehicle_controller.run_step(0, self._current_waypoint) # return control return None self._target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # for i in range(len(self._waypoint_buffer)): # wp, _ = self._waypoint_buffer[i] # print("waypoint in buffer is ", wp) # move using PID controllers # print("target_waypoint: ", self._target_waypoint) control = self._vehicle_controller.run_step(self._target_speed, self._target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # print("len of buffer: ", len(self._waypoint_buffer)) # for i in range(len(self._waypoint_buffer)): # waypoint = self for i, (waypoint, _) in enumerate(self._waypoint_buffer): distance = distance_vehicle(waypoint, vehicle_transform) # print("min distance: ", self._min_distance, "distance: ", distance) # time.sleep(2) # 当前车辆和路点的距离小于最小距离,认为已经行驶完成 if distance < self._min_distance: # print("waypoint in enumerate is ", waypoint) max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() self.finished_waypoints += 1 print("current finished waypoints is ", self.finished_waypoints) if debug: draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0) # if len(self._waypoint_buffer) == 0: # self.finished_waypoints = self.message_waypoints return control
def run_step(self, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ ##718 remove waypoints until the closest one vehicle_transform = self._vehicle.get_transform() min_index = -1 min_dist = 99999 for i, (waypoint, _) in enumerate(self._waypoint_buffer): dist = distance_vehicle(waypoint, vehicle_transform) if (dist < min_dist): min_dist = dist min_index = i #max_index = i if min_index >= 0: for i in range(min_index): self._waypoint_buffer.popleft() # not enough waypoints in the horizon? => add more! if not self._global_plan and len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): self._compute_next_waypoints(k=100) if len(self._waypoints_queue) == 0: ## #print('_waypoints_queue empty') control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control ## #print(self._waypoint_buffer) # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # target waypoint self.target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # move using PID controllers control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) ##testing waypoints #if self.target_waypoint: #print('target_waypoint',self.target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # find the waypoint in waypt buffer that have min distance # and pop anything before that for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: #sett = self._vehicle.get_world().get_settings() #print(sett) draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) #draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) return control
def run_step(self, target_speed=None, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param target_speed: desired speed :param debug: boolean flag to activate waypoints debugging :return: control """ if target_speed is not None: self._target_speed = target_speed else: self._target_speed = self._vehicle.get_speed_limit() # Buffer waypoints self._buffer_waypoints() if len(self._waypoint_buffer) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) speed = get_speed(self._vehicle) # kph look_ahead = max(2, speed / 4.5) # Target waypoint self.target_waypoint, self.target_road_option = self._waypoint_buffer[ 0] look_ahead_loc = self._get_look_ahead_location(look_ahead) if target_speed > 50: args_lat = self.args_lat_hw_dict args_long = self.args_long_hw_dict else: args_lat = self.args_lat_city_dict args_long = self.args_long_city_dict if not self._pid_controller: self._pid_controller = VehiclePIDController( self._vehicle, args_lateral=args_lat, args_longitudinal=args_long) else: self._pid_controller.set_lon_controller_params(**args_long) self._pid_controller.set_lat_controller_params(**args_lat) control = self._pid_controller.run_step(self._target_speed, look_ahead_loc) # Purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): if i == max_index: self._prev_waypoint = self._waypoint_buffer.popleft()[0] else: self._waypoint_buffer.popleft() if debug: carla_world = self._vehicle.get_world() # Draw current buffered waypoint buffered_waypts = [elem[0] for elem in self._waypoint_buffer] draw_waypoints(carla_world, buffered_waypts) # Draw current look ahead point look_ahead_loc carla_world.debug.draw_line(look_ahead_loc, look_ahead_loc + carla.Location(z=0.2), color=carla.Color(255, 255, 0), thickness=0.2, life_time=1.0) return control
def run_step(self, target_speed=None, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param target_speed: desired speed :param debug: boolean flag to activate waypoints debugging :return: control """ if target_speed is not None: self._target_speed = target_speed else: self._target_speed = self._vehicle.get_speed_limit() if len(self.waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self.waypoints_queue: self._waypoint_buffer.append( self.waypoints_queue.popleft()) else: break # Current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # Target waypoint self.target_waypoint, self.target_road_option = self._waypoint_buffer[ 0] if target_speed > 50: args_lat = self.args_lat_hw_dict args_long = self.args_long_hw_dict else: args_lat = self.args_lat_city_dict args_long = self.args_long_city_dict self._pid_controller = VehiclePIDController( self._vehicle, args_lateral=args_lat, args_longitudinal=args_long) control = self._pid_controller.run_step(self._target_speed, self.target_waypoint) # Purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) return control
def done(self): vehicle_transform = self._vehicle.get_transform() return len(self._waypoints_queue) == 0 and all([ distance_vehicle(wp, vehicle_transform) < self._min_distance for wp in self._waypoints_queue ])
def run_step(self, dest, lane_change, world, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ # if(lane_change == "right"): # print('switch into the right lane') # # change target waypoint to a point on the right lane # right_lane = self._current_waypoint.get_right_lane() # new_waypoint = right_lane.next(5)[0] # # self.target_waypoint = new_waypoint # self._waypoints_queue.clear() # self._waypoint_buffer.clear() # self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW)) # elif(lane_change == "left"): # print('switch into the left lane') # # change target waypoint to a point on the right lane # left_lane = self._current_waypoint.get_left_lane() # new_waypoint = left_lane.next(5)[0] # # self.target_waypoint = new_waypoint # self._waypoints_queue.clear() # self._waypoint_buffer.clear() # print(len(self._waypoints_queue)) # # print("new waypoint at " + str(new_waypoint)) # self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW)) # not enough waypoints in the horizon? => add more! if not self._global_plan and len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): # print("current waypoint: " + str(self._current_waypoint)) # print("add 100 additional waypoints") self._compute_next_waypoints(dest, k=100) current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) if len(self._waypoints_queue) == 0: if not current_waypoint.is_intersection: self._target_road_option = RoadOption.LANEFOLLOW print("way points queue is empty") control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # target waypoint self.target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # print("target waypoint at " + str(self.target_waypoint)) if (lane_change == "right"): # print('switch into the right lane') # change target waypoint to a point on the right lane right_lane = self._current_waypoint.get_right_lane() if (not right_lane): print("cannot switch into the right lane") else: self.target_waypoint = right_lane.next(5)[-1] # self.target_waypoint = new_waypoint elif (lane_change == "left"): # print('switch into the left lane') # change target waypoint to a point on the right lane left_lane = self._current_waypoint.get_left_lane() if (not left_lane): print("cannot switch into the right lane") else: self.target_waypoint = left_lane.next(5)[-1] # # set the target speed # speed_limit = self._vehicle.get_speed_limit() # #set highway driving speed to 40 kmh # if(speed_limit > 30): # self.set_speed(30) # # otherwise, set driving speed to 20 kmh # else: # self.set_speed(20) # move using PID controllers #print("target_speed: " + str(self._target_speed)) control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() # if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) # if self._target_road_option != RoadOption.LANEFOLLOW not current_waypoint.is_intersection: # self._target_road_option = RoadOption.LANEFOLLOW return control
def run_step(self, timestep: int, debug=True, log=False): """ Execute one step of classic mpc controller which follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ start_time = time.time() # not enough waypoints in the horizon? => Sample new ones self._sampling_radius = calculate_step_distance( get_speed(self._vehicle), 0.2, factor=2) # factor 11 --> prediction horizon 10 steps if self._sampling_radius < 2: self._sampling_radius = 3 # Getting future waypoints self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._next_wp_queue = compute_next_waypoints(self._current_waypoint, d=self._sampling_radius, k=20) # Getting waypoint history --> history somehow starts at last wp of future wp self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._previous_wp_queue = compute_previous_waypoints( self._current_waypoint, d=self._sampling_radius, k=5) # concentrate history, current waypoint, future self._waypoint_buffer = deque(maxlen=self._buffer_size) self._waypoint_buffer.extendleft(self._previous_wp_queue) self._waypoint_buffer.append( (self._map.get_waypoint(self._vehicle.get_location()), RoadOption.LANEFOLLOW)) self._waypoint_buffer.extend(self._next_wp_queue) self._waypoints_queue = self._next_wp_queue # If no more waypoints in queue, returning emergency braking control if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False print("Applied emergency control !!!") return control # target waypoint for Frenet calculation self.wp1 = self._map.get_waypoint(self._vehicle.get_location()) self.wp2, _ = self._next_wp_queue[0] # current vehicle waypoint self.kappa_log = dict() self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self.curv_args, self.kappa_log = self.calculate_curvature_func_args( log=True) self.curv_x0 = func_kappa(0, self.curv_args) # input for MPC controller --> wp_current, wp_next, kappa target_wps = [self.wp1, self.wp2, self.curv_x0] # move using MPC controller if log: if timestep / 30 > 8 and timestep / 30 < 9: # Setting manual control manual_control = [self.data_log.get('u_acceleration'), 0.01] target_wps.append(manual_control) print(manual_control) # apply manual control control, state, u, x_log, u_log, _ = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=False, manual=True, log=log) self.data_log = { 'X': state[0], 'Y': state[1], 'PSI': state[2], 'Velocity': state[3], 'Xi': state[4], 'Eta': state[5], 'Theta': state[6], 'u_acceleration': u[0], 'u_steering_angle': u[1], 'pred_states': [x_log], 'pred_control': [u_log], 'computation_time': time.time() - start_time, "kappa": self.curv_x0, "curvature_radius": 1 / self.curv_x0 } elif timestep / 30 > 14 and timestep / 30 < 14.6: # Setting manual control manual_control = [self.data_log.get('u_acceleration'), -0.02] target_wps.append(manual_control) # apply manual control control, state, u, x_log, u_log, _ = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=False, manual=True, log=log) self.data_log = { 'X': state[0], 'Y': state[1], 'PSI': state[2], 'Velocity': state[3], 'Xi': state[4], 'Eta': state[5], 'Theta': state[6], 'u_acceleration': u[0], 'u_steering_angle': u[1], 'pred_states': [x_log], 'pred_control': [u_log], 'computation_time': time.time() - start_time, "kappa": self.curv_x0, "curvature_radius": 1 / self.curv_x0 } else: if timestep % 6 == 0: control, state, u, u_log, x_log, _ = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=True, log=log) # Updating logging information of the logger self.data_log = { 'X': state[0], 'Y': state[1], 'PSI': state[2], 'Velocity': state[3], 'Xi': state[4], 'Eta': state[5], 'Theta': state[6], 'u_acceleration': u[0], 'u_steering_angle': u[1], 'pred_states': [x_log], 'pred_control': [u_log], 'computation_time': time.time() - start_time, "kappa": self.curv_x0, "curvature_radius": 1 / self.curv_x0 } else: control, state, prediction, u = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=False, log=log) # Updating logging information of the logger self.data_log = { 'X': state[0], 'Y': state[1], 'PSI': state[2], 'Velocity': state[3], 'Xi': state[4], 'Eta': state[5], 'Theta': state[6], 'u_acceleration': u[0], 'u_steering_angle': u[1], 'pred_states': [prediction], 'pred_control': self.data_log.get("pred_control"), 'computation_time': time.time() - start_time, "kappa": self.curv_x0, "curvature_radius": 1 / self.curv_x0 } else: if timestep % 6 == 0: control = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=True, log=False) else: control, _, _, _ = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=False, log=log) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: last_wp, _ = self._waypoint_buffer[len(self._waypoint_buffer) - 1] draw_waypoints(self._vehicle.get_world(), [self.wp1, self.wp2], self._vehicle.get_location().z + 1.0) draw_waypoints_debug(self._vehicle.get_world(), [last_wp], self._vehicle.get_location().z + 1.0, color=(0, 255, 0)) return control, self.data_log, self.kappa_log if log else control
def is_valid_lane_change(road_option, world, proximity_threshold=5.9): if road_option != RoadOption.CHANGELANELEFT and road_option != RoadOption.CHANGELANERIGHT: print("ERROR: road option is not a lane change") return False player_waypoint = world.map.get_waypoint(world.player.get_location()) player_lane_id = player_waypoint.lane_id player_transform = world.player.get_transform() player_yaw = player_transform.rotation.yaw vehicle_list = world.world.get_actors().filter('vehicle.*') # Lane change left if road_option == RoadOption.CHANGELANELEFT: # check if lane change is valid if not player_waypoint.lane_change & carla.LaneChange.Left: #print("Traffic rules does not allow left lane change here") return False # Only look at vehicles in left adjecent lane for vehicle in vehicle_list: #vehicle = actorvehicle_list_list.find(vehicle_id) vehicle_lane_id = world.map.get_waypoint( vehicle.get_location()).lane_id # Check if lane_id is in the same driving direction if (player_lane_id < 0 and vehicle_lane_id < 0) or (player_lane_id > 0 and vehicle_lane_id > 0): if abs(player_lane_id) - abs(vehicle_lane_id) == 1: # check the vehicle|s proximity to the player vehicle_waypoint = world.map.get_waypoint( vehicle.get_location()) if distance_vehicle( vehicle_waypoint, player_transform) < proximity_threshold: return False # Lane change right else: # check if lane change is valid if not player_waypoint.lane_change & carla.LaneChange.Right: #print("Traffic rules does not allow right lane change here") return False # Only look for vehicles in right adjencent lane for vehicle in vehicle_list: #vehicle = vehicle_list.find(vehicle_id) vehicle_lane_id = world.map.get_waypoint( vehicle.get_location()).lane_id # Check if lane_id is in the same driving direction if (player_lane_id < 0 and vehicle_lane_id < 0) or (player_lane_id > 0 and vehicle_lane_id > 0): if abs(player_lane_id) - abs(vehicle_lane_id) == -1: # check the vehicle|s proximity to the player vehicle_waypoint = world.map.get_waypoint( vehicle.get_location()) if distance_vehicle( vehicle_waypoint, player_transform) < proximity_threshold: return False return True
def run_step(self, debug=True): # not enough waypoints in the horizon? => add more! if len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): if not self._global_plan: self._compute_next_waypoints(k=100) if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control veh_location = self._vehicle.get_location() # type: carla.Location veh_waypoint = self._map.get_waypoint( veh_location) # type: carla.Waypoint veh_yaw = self._vehicle.get_transform( ).rotation.yaw # TODO type: float, range TODO local_plan = self.get_filled_waypoint_buffer( ) # type: List[carla.Waypoint] # Calculate best waypoint to follow considering current yaw L = 2.9 fx = veh_location.x + L * np.cos(veh_yaw) fy = veh_location.y + L * np.sin(veh_yaw) distances = [] for waypoint in local_plan: wp = waypoint.transform.location dx = fx - wp.x dy = fy - wp.y distance = np.sqrt(dx**2 + dy**2) distances.append(distance) target_idx = np.argmin(distances) closest_error = distances[target_idx] self._target_waypoint = local_plan[target_idx] # Calculate path curvature waypoints_to_look_ahead = 6 reference_waypoint = local_plan[target_idx + waypoints_to_look_ahead] ref_location = reference_waypoint.transform.location # delta_x = ref_location.x - veh_location.x # delta_y = ref_location.y - veh_location.y # theta_radians = math.atan2(delta_y, delta_x) # FIXME Sometimes yaw oscilates from 179 to -179 which leads to temporarily bad calculations distance, relative_angle = compute_magnitude_angle( ref_location, veh_location, veh_yaw) #np.rad2deg(theta_radians) - veh_yaw # plt.cla() # plt.plot([self._vehicle.get_transform().location.x, lookahead_waypoint.transform.location.x], [self._vehicle.get_transform().location.y, lookahead_waypoint.transform.location.y], "-r", label="debug") # # plt.plot(, , ".b", label="lookahead") # plt.axis("equal") # plt.legend() # plt.grid(True) # plt.title("Rel angle: {}, yaw {}".format(str(angle), yaw)) # plt.pause(0.0001) if abs(relative_angle) < 15: target_speed = 50 elif abs(relative_angle) < 20: target_speed = 40 elif abs(relative_angle) < 30: target_speed = 30 else: target_speed = 20 print( 'Relative angle to reference waypoint: {:3d} | Vehicle yaw angle: {:3d} | Target speed {} km/h' .format(int(relative_angle), int(veh_yaw), target_speed)) control = self._vehicle_controller.run_step(target_speed, self._target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # i, (waypoint, _) for i, waypoint in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: #draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0, color=carla.Color(0, 255, 0)) draw_waypoints(self._vehicle.get_world(), [reference_waypoint], veh_location.z + 1.0) return control
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.')