def read_agents_from_file(self, filename):
wb = xlrd.open_workbook(filename)
sheet = wb.sheet_by_index(0)
self.coordinates = []
self.colors = []
self.max_speeds = []
for i in range(1, sheet.nrows):
# agent_id = sheet.cell_value(i, 0)
pos = sheet.cell_value(i, 1).split(',')
color = sheet.cell_value(i, 2).split(',')
max_speed = float(sheet.cell_value(i, 3))
pos_x, pos_y = float(pos[0]), float(pos[1])
pos = Vector2(pos_x, pos_y)
color = torch.tensor([int(color[0]), int(color[1]), int(color[2])])
self.add_agent(pos)
self.agents_[i-1].max_speed_ = max_speed
self.agents_[i-1].color_ = np.array(color)
self.coordinates.append(pos_x)
self.coordinates.append(pos_y)
self.colors.append(color)
self.max_speeds.append(max_speed)
dists = self.dist_to_goals(pos, self.goals[i-1])
self.d_prev.append(dists)
self.v_prev.append(Vector2(0., 0.))
print('load done')
def reset_room(self):
index = 0
while index < self.num_agents:
self.delete_agent(index)
if self.open_time == 0:
self.goals = [[self.exit1] for _ in range(glo_num_agents)]
else:
self.goals = [[self.exit1, self.exit2]
for _ in range(glo_num_agents)]
self.global_time_ = 0
self.kd_tree_.agents_ = None
self.kd_tree_.agentTree_ = None
self.d_prev = []
self.v_prev = []
# self.arrived = [False for _ in range(glo_num_agents)]
# self.random_agents(glo_num_agents, glo_center, glo_border, glo_wall_width)
for i in range(glo_num_agents):
pos = Vector2(self.coordinates[i*2], self.coordinates[i*2+1])
self.add_agent(pos)
self.agents_[i].max_speed_ = Max_Speed # self.max_speeds[i]
self.agents_[i].color_ = self.colors[i]
self.arrived.append(False)
dists = self.dist_to_goals(pos, self.goals[i])
self.d_prev.append(dists)
self.v_prev.append(Vector2(0., 0.))
def random_agents(self, num_agents, center, border, w_w, ratio=[1, 1]):
self.coordinates = []
self.max_speeds = np.random.random(num_agents) * 3. + Max_Speed
exit_1 = (int)(ratio[0] * num_agents / sum(ratio))
# (int)(ratio[1] * num_agents / sum(ratio))
exit_2 = num_agents - exit_1
exits_count = [exit_1, exit_2]
ratio_count = [0, 0]
i = 0
while i < (exit_1+exit_2):
coord = center[0]-border[0]+w_w*3 + \
np.random.random(2) * (border[0]*2-w_w*6)
pos = Vector2(coord[0], coord[1])
dists = self.dist_to_goals(pos, self.goals[i])
dists = np.array(dists)
min_index = dists.argmin()
if ratio_count[min_index] < exits_count[min_index]:
ratio_count[min_index] += 1
self.add_agent(pos)
self.colors.append(np.random.randint(0, 255, 3))
self.agents_[i].max_speed_ = self.max_speeds[i]
self.agents_[i].color_ = self.colors[i]
self.d_prev.append(dists)
self.v_prev.append(Vector2(0., 0.))
self.coordinates.append(coord[0])
self.coordinates.append(coord[1])
i += 1
self.save_agents('agents_info.xls')
def setup_scenario(self):
# Specify the global time step of the simulation.
self.simulator_.set_time_step(0.25)
# Specify the default parameters for agents that are subsequently added.
self.simulator_.set_agent_defaults(15.0, 10, 10.0, 10.0, 1.5, 2.0, Vector2(0.0, 0.0))
# Add agents, specifying their start position, and store their goals on the opposite side of the environment.
for i in range(250):
self.simulator_.add_agent(200.0 *
Vector2(math.cos(i * 2.0 * math.pi / 250.0), math.sin(i * 2.0 * math.pi / 250.0)))
self.goals_.append(-self.simulator_.agents_[i].position_)
def action_to_vector_discrete(pos, target, direction, prev):
# go straight if near to the goal
# min_dist = np.array(prev).min()
# if min_dist < Radius * 8:
# min_index = np.array(prev).argmax()
# directV = rvo_math.normalize(target[min_index]-pos)
# return directV * Max_Speed
V_pref = Vector2(0.0, 0.0)
# rvo_math.normalize(target[0]-pos) # Vector2(0., 1.) #
directV = Vector2(0., 1.)
if direction == Direction.Forward.value:
V_pref = directV
elif direction == Direction.Backward.value:
V_pref = -directV
elif direction == Direction.FR.value:
theta = -math.pi / 4.
x1, y1 = directV.x, directV.y
x2 = math.cos(theta) * x1 - math.sin(theta) * y1
y2 = math.sin(theta) * x1 + math.cos(theta) * y1
V_pref = Vector2(x2, y2)
elif direction == Direction.FL.value:
theta = math.pi / 4
x1, y1 = directV.x, directV.y
x2 = math.cos(theta) * x1 - math.sin(theta) * y1
y2 = math.sin(theta) * x1 + math.cos(theta) * y1
V_pref = Vector2(x2, y2)
elif direction == Direction.BR.value:
theta = -3*math.pi / 4.
x1, y1 = directV.x, directV.y
x2 = math.cos(theta) * x1 - math.sin(theta) * y1
y2 = math.sin(theta) * x1 + math.cos(theta) * y1
V_pref = Vector2(x2, y2)
elif direction == Direction.BL.value:
theta = 3*math.pi / 4
x1, y1 = directV.x, directV.y
x2 = math.cos(theta) * x1 - math.sin(theta) * y1
y2 = math.sin(theta) * x1 + math.cos(theta) * y1
V_pref = Vector2(x2, y2)
elif direction == Direction.Right.value:
theta = -math.pi / 2.
x1, y1 = directV.x, directV.y
x2 = math.cos(theta) * x1 - math.sin(theta) * y1
y2 = math.sin(theta) * x1 + math.cos(theta) * y1
V_pref = Vector2(x2, y2)
elif direction == Direction.Left.value:
theta = math.pi / 2
x1, y1 = directV.x, directV.y
x2 = math.cos(theta) * x1 - math.sin(theta) * y1
y2 = math.sin(theta) * x1 + math.cos(theta) * y1
V_pref = Vector2(x2, y2)
else:
V_pref = Vector2(0., 0.)
return V_pref * Max_Speed
def action_to_vector(pos, target, action, max_speed, ORCA=False):
angle, speed = action[0], max_speed
heading = rvo_math.normalize(target-pos)
# if ORCA:
# heading = rvo_math.normalize(target-pos)
# else:
# heading = Vector2(1., 0.) # rvo_math.normalize(target-pos)
# angle = -math.pi + random.random() * 2 * math.pi
x1, y1 = heading.x, heading.y
x2 = math.cos(angle) * x1 - math.sin(angle) * y1
y2 = math.sin(angle) * x1 + math.cos(angle) * y1
return speed * Vector2(x2, y2)
def set_preferred_velocities(self):
# Set the preferred velocity to be a vector of unit magnitude (speed) in the direction of the goal.
for i in range(self.simulator_.num_agents):
goal_vector = self.goals_[i] - self.simulator_.agents_[i].position_
if rvo_math.abs_sq(goal_vector) > 1.0:
goal_vector = rvo_math.normalize(goal_vector)
self.simulator_.set_agent_pref_velocity(i, goal_vector)
# Perturb a little to avoid deadlocks due to perfect symmetry.
angle = random.random() * 2.0 * math.pi
dist = random.random() * 0.0001
self.simulator_.set_agent_pref_velocity(
i, self.simulator_.agents_[i].pref_velocity_ +
dist * Vector2(math.cos(angle), math.sin(angle)))
def setup_scenario(self, num_agents, load_agents=False, read_agents=False):
self.set_time_step(0.5)
self.set_agent_defaults(Radius*5, 5, 5.0, 5.0, Radius,
Max_Speed, Vector2(0., 0.))
extent = (-100., -100., 100., 100.)
if self.scenario == Scenario.One_Exit:
extent = self.init_single_exit_room(
num_agents, load_agents=load_agents, read_agents=read_agents)
elif self.scenario == Scenario.Two_Exits:
extent = self.init_two_exit_room(
num_agents, load_agents=load_agents, read_agents=read_agents)
# view
self.view = Map_Screen(
extent[0], extent[2], extent[1], extent[3], width, height)
# 转换坐标之后再画
img = np.ones((width, height, 3), np.uint8) * 0
img = self.draw_polygon(img)
cv2.imwrite('screenshots/background.png', img)
def setup_scenario(self):
# Specify the global time step of the simulation.
self.simulator_.set_time_step(0.25)
# Specify the default parameters for agents that are subsequently added.
self.simulator_.set_agent_defaults(15.0, 10, 5.0, 5.0, 2.0, 2.0,
Vector2(0.0, 0.0))
# Add agents, specifying their start position, and store their goals on the opposite side of the environment.
for i in range(5):
for j in range(5):
self.simulator_.add_agent(
Vector2(55.0 + i * 10.0, 55.0 + j * 10.0))
self.goals_.append(Vector2(-75.0, -75.0))
self.simulator_.add_agent(
Vector2(-55.0 - i * 10.0, 55.0 + j * 10.0))
self.goals_.append(Vector2(75.0, -75.0))
self.simulator_.add_agent(
Vector2(55.0 + i * 10.0, -55.0 - j * 10.0))
self.goals_.append(Vector2(-75.0, 75.0))
self.simulator_.add_agent(
Vector2(-55.0 - i * 10.0, -55.0 - j * 10.0))
self.goals_.append(Vector2(75.0, 75.0))
# Add (polygonal) obstacles, specifying their vertices in counterclockwise order.
obstacle1 = []
obstacle1.append(Vector2(-10.0, 40.0))
obstacle1.append(Vector2(-40.0, 40.0))
obstacle1.append(Vector2(-40.0, 10.0))
obstacle1.append(Vector2(-10.0, 10.0))
self.simulator_.add_obstacle(obstacle1)
self.obstacles_.append(obstacle1)
obstacle2 = []
obstacle2.append(Vector2(10.0, 40.0))
obstacle2.append(Vector2(10.0, 10.0))
obstacle2.append(Vector2(40.0, 10.0))
obstacle2.append(Vector2(40.0, 40.0))
self.simulator_.add_obstacle(obstacle2)
self.obstacles_.append(obstacle2)
obstacle3 = []
obstacle3.append(Vector2(10.0, -40.0))
obstacle3.append(Vector2(40.0, -40.0))
obstacle3.append(Vector2(40.0, -10.0))
obstacle3.append(Vector2(10.0, -10.0))
self.simulator_.add_obstacle(obstacle3)
self.obstacles_.append(obstacle3)
obstacle4 = []
obstacle4.append(Vector2(-10.0, -40.0))
obstacle4.append(Vector2(-10.0, -10.0))
obstacle4.append(Vector2(-40.0, -10.0))
obstacle4.append(Vector2(-40.0, -40.0))
self.simulator_.add_obstacle(obstacle4)
self.obstacles_.append(obstacle4)
# Process the obstacles so that they are accounted for in the simulation.
self.simulator_.process_obstacles()
def screen_to_map(self, screen):
map_x = self.scale_x*screen[0] + self.map_minX
map_y = self.scale_y*(self.win_y-screen[1]) + self.map_minY
return Vector2(map_x, map_y)
def init_two_exit_room(self, num_agents, width_=100, height_=100, load_agents=True, read_agents=False):
inner_width = width_ * 0.8
inner_height = height_ * 0.8
wall_width = Radius * 1.5
origin_eixt_width = (2.0 * Radius) * 2.0
# 1.0 is a hyperparameter, which to define the different of two exits
another_exit_width = origin_eixt_width * 1.0
half_inner_width, half_inner_height = inner_width / 2., inner_height / 2.
self.walls = []
wall1 = [] # left top
wall2 = [] # left bottom
wall3 = [] # bottom left
wall4 = [] # bottom right
wall5 = [] # top
wall6 = [] # right
half_left_exit_width = another_exit_width / 2.0
wall1.append(Vector2(-half_inner_width -
wall_width, half_left_exit_width))
wall1.append(Vector2(-half_inner_width, half_left_exit_width))
wall1.append(Vector2(-half_inner_width, half_inner_height))
wall1.append(Vector2(-half_inner_width-wall_width, half_inner_height))
self.walls.append(wall1)
wall2.append(Vector2(-half_inner_width-wall_width, -half_inner_height))
wall2.append(Vector2(-half_inner_width, -half_inner_height))
wall2.append(Vector2(-half_inner_width, -half_left_exit_width))
wall2.append(Vector2(-half_inner_width -
wall_width, -half_left_exit_width))
self.walls.append(wall2)
half_bottom_exit_width = origin_eixt_width / 2.0
wall3.append(Vector2(-half_inner_width-wall_width, -
half_inner_height-wall_width))
wall3.append(Vector2(-half_bottom_exit_width, -
half_inner_height-wall_width))
wall3.append(Vector2(-half_bottom_exit_width, -half_inner_height))
wall3.append(Vector2(-half_inner_width-wall_width, -half_inner_height))
self.walls.append(wall3)
wall4.append(Vector2(half_bottom_exit_width, -
half_inner_height-wall_width))
wall4.append(Vector2(half_inner_width+wall_width, -
half_inner_height-wall_width))
wall4.append(Vector2(half_inner_width+wall_width, -half_inner_height))
wall4.append(Vector2(half_bottom_exit_width, -half_inner_height))
self.walls.append(wall4)
wall5.append(Vector2(-half_inner_width-wall_width, half_inner_height))
wall5.append(Vector2(half_inner_width+wall_width, half_inner_height))
wall5.append(Vector2(half_inner_width+wall_width,
half_inner_height+wall_width))
wall5.append(Vector2(-half_inner_width-wall_width,
half_inner_height+wall_width))
self.walls.append(wall5)
wall6.append(Vector2(half_inner_width, -half_inner_height))
wall6.append(Vector2(half_inner_width+wall_width, -half_inner_height))
wall6.append(Vector2(half_inner_width+wall_width, half_inner_height))
wall6.append(Vector2(half_inner_width, half_inner_height))
self.walls.append(wall6)
self.add_obstacle(wall1)
self.add_obstacle(wall2)
self.add_obstacle(wall3)
self.add_obstacle(wall4)
self.add_obstacle(wall5)
self.add_obstacle(wall6)
self.process_obstacles()
self.exit1 = Vector2(0., -half_inner_height-wall_width-5.0)
self.exit2 = Vector2(-half_inner_width-wall_width-5.0, 0.)
self.goals = [[self.exit1, self.exit2] for _ in range(num_agents)]
if load_agents:
if read_agents:
self.read_agents_from_file('agents_info.xls')
else:
global glo_center, glo_border, glo_wall_width
glo_center = [0., 0.]
glo_border = [half_inner_width, half_inner_height]
glo_wall_width = wall_width
self.random_agents(num_agents, glo_center,
glo_border, wall_width)
map_min_x, map_min_y = -width_/2., -height_/2.
map_max_x, map_max_y = width_/2., height_/2.
return (map_min_x, map_min_y, map_max_x, map_max_y)