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" \
+ "........................."
