def control_loop(self): steer, gas = 0,0 if self.feed_u is None: for car in reversed(self.cars): car.control(steer, gas) else: for car, fu, hu in zip(self.cars, self.feed_u, self.history_u): car.u = fu[len(hu)] for car, hist in zip(self.cars, self.history_u): hist.append(car.u) for car in self.cars: self.prev_x[car] = car.x for car in self.cars: print("CAR: {}".format(car.x)) car.move() for car, hist in zip(self.cars, self.history_x): hist.append(car.x) self.prev_t = time.time() self.termination_counter += 1 if self.termination_counter >= self.MAX_ITER: data2_filename = "data2/CDC_RUN_NUM_CARS{0}_AUTLEVEL{1}_TIME{2}.txt".\ format(len(self.cars), self.aut_level, time.time()) write_string = "" for car in self.cars: cur_write = {'lane': car.x[0], 'pos': car.x[1], 'isRobot': car.iamrobot} write_string += str(cur_write) write_string += "\n" with open(data2_filename, 'w') as f: f.write(write_string) #increment the counter of how many control loop iterations since the last midlevel iteration self.cntrl_loop_counter += 1
def control_loop(self, _=None): #print "Time: ", time.time() if self.paused: return if self.iters is not None and len(self.history_x[0]) >= self.iters: if self.autoquit: self.event_loop.exit() return if self.feed_u is not None and len(self.history_u[0]) >= len( self.feed_u[0]): if self.autoquit: self.event_loop.exit() return if self.pause_every is not None and self.pause_every > 0 and len( self.history_u[0]) % self.pause_every == 0: self.paused = True steer = 0. gas = 0. if self.keys[key.UP]: gas += 1. if self.keys[key.DOWN]: gas -= 1. if self.keys[key.LEFT]: steer += 1.5 if self.keys[key.RIGHT]: steer -= 1.5 if self.joystick: steer -= self.joystick.x * 3. gas -= self.joystick.y self.heatmap_valid = False for car in self.cars: self.prev_x[car] = car.x if self.feed_u is None: for car in reversed(self.cars): car.control(steer, gas) else: for car, fu, hu in zip(self.cars, self.feed_u, self.history_u): car.u = fu[len(hu)] for car, hist in zip(self.cars, self.history_u): hist.append(car.u) for car in self.cars: car.move() for car, hist in zip(self.cars, self.history_x): hist.append(car.x) if self.show_feature_sums: self.prev_t = time.time() self.ticks_run += 1 self.feature_sum = np.add( self.feature_sum, self.feature_function(0, self.main_car.x, self.main_car.u, self.main_car)) print self.ticks_run print self.feature_sum
def control_loop(self, _=None): #if not self.paused: #pyglet.image.get_buffer_manager().get_color_buffer().save('screenshots/screenshot-%.2f.png'%time.time()) if self.paused: return if self.iters is not None and len(self.history_x[0])>=self.iters: if self.autoquit: self.event_loop.exit() return if self.feed_u is not None and len(self.history_u[0])>=len(self.feed_u[0]): if self.autoquit: self.event_loop.exit() return if self.pause_every is not None and self.pause_every>0 and len(self.history_u[0])%self.pause_every==0: self.paused = True steer = 0. gas = 0. if self.keys[key.UP]: gas += 1. if self.keys[key.DOWN]: gas -= 1. if self.keys[key.LEFT]: steer += 1.5 if self.keys[key.RIGHT]: steer -= 1.5 if self.joystick: print(self.joystick.y) steer -= self.joystick.x*4. gas -= (self.joystick.y - 1)/2 self.heatmap_valid = False for car in self.cars: self.prev_x[car] = car.x if self.feed_u is None: for car in reversed(self.cars): car.control(steer, gas) else: for car, fu, hu in zip(self.cars, self.feed_u, self.history_u): car.u = fu[len(hu)] copy_cars = set([c for c in self.cars if type(c) is CopyCar]) for car, hist in zip(self.cars, self.history_u): hist.append(car.u) if car not in copy_cars: for cc in copy_cars: cc.use_also(car.x, car.u) for car in self.cars: car.move() for car, hist in zip(self.cars, self.history_x): print(car.x) hist.append(car.x) for car, hist in zip(self.cars, self.history_belief): if hasattr(car, 'log_ps'): hist.append(np.asarray([np.exp(log_p.get_value()) for log_p in car.log_ps])) self.prev_t = time.time()
def keyPressEvent(self, event): key = event.key() if key == Qt.Key_A and not event.isAutoRepeat(): car.turn('left') print('left') elif key == Qt.Key_D and not event.isAutoRepeat(): car.turn('right') print('right') elif key == Qt.Key_S and not event.isAutoRepeat(): car.move('back') print('back') elif key == Qt.Key_W and not event.isAutoRepeat(): car.move('front') print('front')
def control_loop(self, _=None): if self.paused: return if self.iters is not None and len(self.history_x[0]) >= self.iters: if self.autoquit: self.event_loop.exit() return if self.feed_u is not None and len(self.history_u[0]) >= len( self.feed_u[0]): if self.autoquit: self.event_loop.exit() return if self.pause_every is not None and self.pause_every > 0 and len( self.history_u[0]) % self.pause_every == 0: self.paused = True steer = 0. gas = 0. if self.keys[key.UP]: gas += 1. if self.keys[key.DOWN]: gas -= 1. if self.keys[key.LEFT]: steer += 1.5 if self.keys[key.RIGHT]: steer -= 1.5 if self.joystick: steer -= self.joystick.x * 3. gas -= self.joystick.y self.heatmap_valid = False for car in self.cars: self.prev_x[car] = car.x if self.feed_u is None: for car in reversed(self.cars): car.control(steer, gas) else: for car, fu, hu in zip(self.cars, self.feed_u, self.history_u): car.u = fu[len(hu)] for car, hist in zip(self.cars, self.history_u): hist.append(car.u) for car in self.cars: car.move() for car, hist in zip(self.cars, self.history_x): hist.append(car.x) for car, hist in zip(self.cars, self.history_belief): if hasattr(car, 'log_ps'): hist.append( np.asarray( [np.exp(log_p.get_value()) for log_p in car.log_ps])) self.prev_t = time.time()
def control_loop(self, _=None): #print "Time: ", time.time() if self.paused: return if self.iters is not None and len(self.history_x[0])>=self.iters: if self.autoquit: self.event_loop.exit() return if self.feed_u is not None and len(self.history_u[0])>=len(self.feed_u[0]): if self.autoquit: self.event_loop.exit() return if self.pause_every is not None and self.pause_every>0 and len(self.history_u[0])%self.pause_every==0: self.paused = True steer = 0. gas = 0. if self.keys[key.UP]: gas += 1. if self.keys[key.DOWN]: gas -= 1. if self.keys[key.LEFT]: steer += 1.5 if self.keys[key.RIGHT]: steer -= 1.5 if self.joystick: steer -= self.joystick.x*3. gas -= self.joystick.y self.heatmap_valid = False for car in self.cars: self.prev_x[car] = car.x if self.feed_u is None: for car in reversed(self.cars): car.control(steer, gas) # IMPORTANT: very slow when using the nested optimizr else: for car, fu, hu in zip(self.cars, self.feed_u, self.history_u): car.u = fu[len(hu)] for car, hist in zip(self.cars, self.history_u): hist.append(car.u) for car in self.cars: car.move() for car, hist in zip(self.cars, self.history_x): hist.append(car.x) self.prev_t = time.time() # use this to get all pictures pyglet.image.get_buffer_manager().get_color_buffer().save('screenshots/screenshot-%.2f.png'%time.time())
def control_loop(self, _=None): #print "Time: ", time.time() if self.paused: return if self.iters is not None and len(self.history_x[0])>=self.iters: if self.autoquit: self.event_loop.exit() return if self.feed_u is not None and len(self.history_u[0])>=len(self.feed_u[0]): if self.autoquit: self.event_loop.exit() return if self.pause_every is not None and self.pause_every>0 and len(self.history_u[0])%self.pause_every==0: self.paused = True steer = 0. gas = 0. if self.keys[key.UP]: gas += 1. if self.keys[key.DOWN]: gas -= 1. if self.keys[key.LEFT]: steer += 1.5 if self.keys[key.RIGHT]: steer -= 1.5 if self.joystick: steer -= self.joystick.x*3. gas -= self.joystick.y self.heatmap_valid = False for car in self.cars: self.prev_x[car] = car.x if self.feed_u is None: for car in reversed(self.cars): car.control(steer, gas) else: for car, fu, hu in zip(self.cars, self.feed_u, self.history_u): car.u = fu[len(hu)] for car, hist in zip(self.cars, self.history_u): hist.append(car.u) for car in self.cars: car.move() for car, hist in zip(self.cars, self.history_x): hist.append(car.x) self.prev_t = time.time()
def calc_next_node(current, steer, direction, config, ox, oy, kdtree): """ 获取按照当前控制策略后到达的情况 :param current: :param steer: :param direction: :param config: :param ox: :param oy: :param kdtree: :return: """ x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1] arc_l = XY_GRID_RESOLUTION * 1.5 xlist, ylist, yawlist = [], [], [] for dist in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) xlist.append(x) ylist.append(y) yawlist.append(yaw) if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): return None d = direction == 1 xind = round(x / XY_GRID_RESOLUTION) yind = round(y / XY_GRID_RESOLUTION) yawind = round(yaw / YAW_GRID_RESOLUTION) addedcost = 0.0 if d != current.direction: addedcost += SB_COST # steer penalty addedcost += STEER_COST * abs(steer) # steer change penalty addedcost += STEER_CHANGE_COST * abs(current.steer - steer) cost = current.cost + addedcost + arc_l node = Node(xind, yind, yawind, d, xlist, ylist, yawlist, [d], pind=calc_index(current, config), cost=cost, steer=steer) return node
def calc_next_node(current, steer, direction, config, ox, oy, kd_tree): # pose of current node x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1] arc_l = XY_GRID_RESOLUTION * 1.5 x_list, y_list, yaw_list = [], [], [] for _ in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) x_list.append(x) y_list.append(y) yaw_list.append(yaw) if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): return None d = direction == 1 # pose finally reached x_ind = round(x / XY_GRID_RESOLUTION) y_ind = round(y / XY_GRID_RESOLUTION) yaw_ind = round(yaw / YAW_GRID_RESOLUTION) added_cost = 0.0 # driving forward-backward change penalty if d != current.direction: added_cost += SB_COST # steer penalty added_cost += STEER_COST * abs(steer) # steer change penalty added_cost += STEER_CHANGE_COST * abs(current.steer - steer) # distance(time) penalty added_cost += arc_l cost = current.cost + added_cost node = Node( x_ind, y_ind, yaw_ind, d, x_list, y_list, yaw_list, [d], parent_index=calc_index(current, config), cost=cost, steer=steer) return node
def calc_next_node(current, steer, direction, config, ox, oy, kd_tree): x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1] # todo 弧长arc_l的选择对路径生成的影响很重要,其取值影响车辆状态积分后的节点落在哪个栅格中,取值越大落在其他栅格中的可能性越大 arc_l = XY_GRID_RESOLUTION * 1.5 # 根据当前车辆状态与离散化的输入,积分推断朝s=arc_l的距离前进产生的状态序列[x,y,yaw] x_list, y_list, yaw_list = [], [], [] for _ in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) x_list.append(x) y_list.append(y) yaw_list.append(yaw) # collision check 碰撞检测 if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): return None d = direction == 1 x_ind = round(x / XY_GRID_RESOLUTION) y_ind = round(y / XY_GRID_RESOLUTION) yaw_ind = round(yaw / YAW_GRID_RESOLUTION) # 相对父节点增加的cost added_cost = 0.0 # 调转车头的penalty(最大) if d != current.direction: added_cost += SB_COST # steer penalty added_cost += STEER_COST * abs(steer) # steer change penalty added_cost += STEER_CHANGE_COST * abs(current.steer - steer) # 父节点的cost + 移动距离 + switch back,steer,steer change penalty cost = current.cost + added_cost + arc_l node = Node(x_ind, y_ind, yaw_ind, d, x_list, y_list, yaw_list, [d], parent_index=calc_index(current, config), cost=cost, steer=steer) return node
def calc_next_node(current, steer, direction, config, ox, oy, kd_tree): x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1] arc_l = XY_GRID_RESOLUTION * 1.5 # arc_l = XY_GRID_RESOLUTION * 0.5 x_list, y_list, yaw_list = [], [], [] for _ in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) # x, y, yaw = self.control_data['target_speed'] x_list.append(x) y_list.append(y) yaw_list.append(yaw) if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): return None d = direction == 1 x_ind = round(x / XY_GRID_RESOLUTION) y_ind = round(y / XY_GRID_RESOLUTION) yaw_ind = round(yaw / YAW_GRID_RESOLUTION) added_cost = 0.0 if d != current.direction: added_cost += SB_COST # steer penalty added_cost += STEER_COST * abs(steer) # steer change penalty added_cost += STEER_CHANGE_COST * abs(current.steer - steer) cost = current.cost + added_cost + arc_l node = Node(x_ind, y_ind, yaw_ind, d, x_list, y_list, yaw_list, [d], parent_index=calc_index(current, config), cost=cost, steer=steer) return node
def run(self): screen = pygame.display.set_mode(self.window_size) stat_font = pygame.font.Font(None, 24) stat_color = (255, 255, 0) stat_pos = (self.window_size[0] - 200, 20) running = True stop_trial = False total_time = 0.0 generation_number = 1 while running: # time_passed = self.clock.tick(self.max_fps) * .001 time_passed = self.clock.tick() * .001 total_time += time_passed # Handle events. for event in pygame.event.get(): if event.type is pygame.QUIT: running = False elif event.type is pygame.KEYDOWN: if event.key is pygame.K_ESCAPE: running = False elif event.key is pygame.K_r: stop_trial = True elif event.type is pygame.MOUSEMOTION or event.type is pygame.MOUSEBUTTONDOWN: pos = event.pos pressed = pygame.mouse.get_pressed() if pressed[0]: # Left mouse button. self.map.set_cell_at(pos, True) elif pressed[2]: # Right mouse button. self.map.set_cell_at(pos, False) # Update the objects. nr_cars_alive = 0 for car in self.cars: car.move(time_passed) car.update(time_passed) if not car.crashed: nr_cars_alive += 1 # Draw the objects. screen.fill(self.background_color) self.map.draw(screen) for car in self.cars: car.draw(screen) fps = stat_font.render("FPS: %3.0f" % self.clock.get_fps(), True, stat_color) screen.blit(fps, stat_pos) time = stat_font.render("Time: %2.0f" % total_time, True, stat_color) screen.blit(time, (stat_pos[0], stat_pos[1] + stat_font.get_linesize())) gen = stat_font.render("Generation: %3d" % generation_number, True, stat_color) screen.blit( gen, (stat_pos[0], stat_pos[1] + 2 * stat_font.get_linesize())) alive = stat_font.render("Alive: %3d" % nr_cars_alive, True, stat_color) screen.blit( alive, (stat_pos[0], stat_pos[1] + 3 * stat_font.get_linesize())) pygame.display.flip() # Check if the current run is over. if total_time >= 20.0 or nr_cars_alive is 0 or stop_trial: stop_trial = False total_time = 0.0 generation_number += 1 self.evolve()
def control_loop(self, _=None): #print "Time: ", time.time() if self.paused: return if self.iters is not None and len(self.history_x[0]) >= self.iters: if self.autoquit: self.event_loop.exit() return if self.feed_u is not None and len(self.history_u[0]) >= len( self.feed_u[0]): if self.autoquit: self.event_loop.exit() return if self.pause_every is not None and self.pause_every > 0 and len( self.history_u[0]) % self.pause_every == 0: self.paused = True steer_arrows = 0. gas_arrows = 0. if self.keys[key.UP]: gas_arrows += 1. if self.keys[key.DOWN]: gas_arrows -= 1. if self.keys[key.LEFT]: steer_arrows += 1.5 if self.keys[key.RIGHT]: steer_arrows -= 1.5 if self.joystick: steer_arrows -= self.joystick.x * 3. gas_arrows -= self.joystick.y steer_wasd = 0. # for cars controlled using the 'wasd' keys gas_wasd = 0. if self.keys[key.W]: gas_wasd += 1. if self.keys[key.S]: gas_wasd -= 1. if self.keys[key.A]: steer_wasd += 1.5 if self.keys[key.D]: steer_wasd -= 1.5 if self.joystick: steer_wasd -= self.joystick.x * 3. gas_wasd -= self.joystick.y self.heatmap_valid = False for car in self.cars: self.prev_x[car] = car.x if self.feed_u is None: for car in reversed(self.cars): if car.controls == 'wasd': car.control(steer_wasd, gas_wasd) else: car.control(steer_arrows, gas_arrows) else: for car, fu, hu in zip(self.cars, self.feed_u, self.history_u): car.u = fu[len(hu)] for car, hist in zip(self.cars, self.history_u): hist.append(car.u) for car in self.cars: car.move() for car, hist in zip(self.cars, self.history_x): hist.append(car.x) self.prev_t = time.time()
def control_loop(self, _=None): #print "Time: ", time.time() if self.paused: return if self.iters is not None and len(self.history_x[0])>=self.iters: if self.autoquit: self.event_loop.exit() return if self.feed_u is not None and len(self.history_u[0])>=len(self.feed_u[0]): if self.autoquit: self.event_loop.exit() return if self.pause_every is not None and self.pause_every>0 and len(self.history_u[0])%self.pause_every==0: self.paused = True steer = 0. gas = 0. if self.keys[key.UP]: gas += 1. if self.keys[key.DOWN]: gas -= 1. if self.keys[key.LEFT]: steer += 1.5 if self.keys[key.RIGHT]: steer -= 1.5 if self.joystick: steer -= self.joystick.x*3. gas -= self.joystick.y self.heatmap_valid = False for car in self.cars: self.prev_x[car] = car.x if self.feed_u is None: for car in reversed(self.cars): car.control(steer, gas) else: for car, fu, hu in zip(self.cars, self.feed_u, self.history_u): car.u = fu[len(hu)] for car, hist in zip(self.cars, self.history_u): hist.append(car.u) for car in self.cars: print("CAR: {}".format(car.x)) car.move() for car, hist in zip(self.cars, self.history_x): hist.append(car.x) self.prev_t = time.time() self.termination_counter += 1 if self.termination_counter >= self.MAX_ITER: print("Freaky things are happening\n\n") #self.paused = True data2_filename = "data2/CDC_RUN_NUM_CARS{0}_AUTLEVEL{1}_TIME{2}.txt".\ format(len(self.cars), self.aut_level, time.time()) write_string = "" for car in self.cars: cur_write = {'lane': car.x[0], 'pos': car.x[1], 'isRobot': car.iamrobot} write_string += str(cur_write) write_string += "\n" with open(data2_filename, 'w') as f: f.write(write_string) self.event_loop.exit() #increment the counter of how many control loop iterations since the last midlevel iteration self.cntrl_loop_counter += 1
def calc_next_node(current, steer, direction, config, ox, oy, kdtree): """ Calculate cost and indices of the node with steer, direction primitives. - current : current node - steer : steer primitives - direction : direction primitives """ x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1] arc_l = XY_GRID_RESOLUTION * 1.5 xlist, ylist, yawlist = [], [], [] for dist in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) xlist.append(x) ylist.append(y) yawlist.append(yaw) if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): return None # d = direction == 1 # check whether direction control input is forward of not. xind = round(x / XY_GRID_RESOLUTION) yind = round(y / XY_GRID_RESOLUTION) yawind = round(yaw / YAW_GRID_RESOLUTION) addedcost = 0.0 # # direction switch penalty # if d != current.direction: # addedcost += SB_COST # direction switch penalty if current.direction != direction: addedcost += SB_COST # backward drive penalty if direction != 1: addedcost += BACK_COST # steer penalty addedcost += STEER_COST * abs(steer) # steer change penalty addedcost += STEER_CHANGE_COST * abs(current.steer - steer) cost = current.cost + addedcost + arc_l node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist, [direction], pind=calc_index(current, config), cost=cost, steer=steer) # node = Node(xind, yind, yawind, d, xlist, # ylist, yawlist, [d], # pind=calc_index(current, config), # cost=cost, steer=steer) return node
car.carStart() pygame.draw.line(screen,settings.red,(car.x+settings.xOffset,car.y+settings.yOffset),(car.x+settings.xOffset,car.y+settings.yOffset),3) #show where we are. car.showDash() #move car while True: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() sys.exit() if car.carRunning == True: car.move() car.showDash() screen.fill(settings.white) map.drawLines(screen,settings.xOffset,settings.yOffset) pygame.draw.circle(screen, settings.red, (car.x+settings.xOffset, car.y+settings.yOffset), 3, 3) matrix = settings.matrix font = pygame.font.Font(None, 26) dashText = "X=: " + str(car.x) + " Y=: " + str(car.y) + " Speed=: " + str(car.speed) + " Direction=:" + str( car.direction) + "\n" \ + str(matrix[car.x - 1][car.y - 1]) + str(matrix[car.x][car.y - 1]) + str( matrix[car.x + 1][car.y - 1]) + "\n" \ + str(matrix[car.x - 1][car.y]) + str(matrix[car.x][car.y]) + str(matrix[car.x + 1][car.y]) + "\n" \ + str(matrix[car.x - 1][car.y + 1]) + str(matrix[car.x][car.y + 1]) + str( matrix[car.x + 1][car.y + 1]) + "\n" \ + "........................."