def search(self, vobj):
frontier = PriorityQueue()
frontier.put(self.start, 0)
came_from = {}
cost_so_far = {}
came_from[self.start] = None
cost_so_far[self.start] = 0
while not frontier.empty():
current = frontier.get()
if current == self.goal:
break
for next in self.graph.neighbors(current):
new_cost = cost_so_far[current] + self.graph.cost(
current, next)
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + self.heuristic(self.goal, next)
frontier.put(next, priority)
came_from[next] = current
# Reconstruct path
path = [current]
while current != self.start:
current = came_from[current]
path.append(current)
nppath = np.asarray(path[::-4]) * self.graph.gridsize
return np.copy(nppath[:])
def travel(dist_mat, startcity=0):
optimal_tour = []
u = Node()
pq = PriorityQueue()
opt_len = 0
v = Node(level=0, path=[0])
min_len = sys.maxsize
v.bound = bound(dist_mat, v)
pq.put(v)
while not pq.empty():
v = pq.get()
if v.bound < min_len:
u.level = v.level + 1
for i in filter(lambda x: x not in v.path, range(1, num_cities)):
u.path = v.path[:]
u.path.append(i)
if u.level == num_cities - 2:
l = set(range(1, num_cities)) - set(u.path)
u.path.append(list(l)[0])
u.path.append(0)
_len = length(dist_mat, u)
if _len < min_len:
min_len = _len
opt_len = _len
optimal_tour = u.path[:]
else:
u.bound = bound(dist_mat, u)
if u.bound < min_len:
pq.put(u)
# make a new node at each iteration!
u = Node(level=u.level)
return optimal_tour, opt_len
def search(self, vobj):
frontier = PriorityQueue()
frontier.put(self.start, 0)
came_from = {}
cost_so_far = {}
came_from[self.start] = None
cost_so_far[self.start] = 0
while not frontier.empty():
current = frontier.get()
if current == self.goal:
break
for next in self.graph.neighbors(current):
new_cost = cost_so_far[current] + self.graph.cost(current, next)
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + self.heuristic(self.goal, next)
frontier.put(next, priority)
came_from[next] = current
# Reconstruct path
path = [current]
while current != self.start:
current = came_from[current]
path.append(current)
nppath = np.asarray(path[::-4]) * self.graph.gridsize
return np.copy(nppath[:])
def a_star_search(self, h_type='zero', visualize=False):
frontier = PriorityQueue() # The OPENLIST
frontier.put(self.start, 0) # PUT START IN THE OPENLIST
parent = {} # parent, {loc: parent}
# g function dict, {loc: f(loc)}, CLOSED LIST BASICALLY
g = {}
parent[self.start] = None
g[self.start] = 0
self.h_type = h_type
if visualize:
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
ax.set_xlim(-100, 100)
ax.set_ylim(-100, 100)
while not frontier.empty():
current = frontier.get(
) # update current to be the item with best priority
if visualize:
ax.plot(current[0], current[1], "xc")
# early exit if we reached our goal
if current == self.goal:
break
for next in self.graph.neighbors(current):
g_next = g[current] + self.graph.cost(current, next)
# if next location not in CLOSED LIST or its cost is less than before
# Newer implementation
if next not in g or g_next < g[next]:
g[next] = g_next
if self.h_type == 'zero':
priority = g_next
else:
priority = g_next + self.heuristic(
self.goal, next, self.h_type)
frontier.put(next, priority)
parent[next] = current
if visualize:
ax.plot(parent[0], parent[1], "-r")
fig.show()
self.parent = parent
self.g = g
return parent, g
def search_path(self, start, goal, heuristic, is_walkable):
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
came_from[start] = None
while not frontier.empty():
current = frontier.get()
if current == goal:
break
for next in self.neighbors(current, is_walkable):
if next not in came_from:
priority = heuristic(goal, next)
frontier.put(next, priority)
came_from[next] = current
if current != goal:
return False
return self.reconstruct_path(came_from, start, goal)
class Simulator(SimulatorBase):
EVENT_QUEUE_THRESHOLD = 100
def __init__(self):
self.modules = {}
self.module_type_map = defaultdict(deque)
self.device_state = DeviceState()
self.event_queue = PriorityQueue()
self.alarms = []
self.modules = {}
self.event_listeners = defaultdict(deque)
self.trace_reader = None
self.verbose = False
self.debug_mode = False
self.debug_interval = 1
self.debug_interval_cnt = 0
def has_module_instance(self, name):
return name in self.modules
def get_module_instance(self, name):
return self.modules[name]
def get_module_for_type(self, module_type):
if module_type in self.module_type_map:
return self.module_type_map[module_type.value][0]
else:
return None
def register(self, sim_module, override=False):
if not isinstance(sim_module, SimModule):
raise TypeError("Expected SimModule object")
if sim_module.get_name() in self.modules:
raise Exception("Module %s already exists" % sim_module.get_name())
self.modules[sim_module.get_name()] = sim_module
if override:
self.module_type_map[sim_module.get_type().value].appendleft(
sim_module)
else:
self.module_type_map[sim_module.get_type().value].append(
sim_module)
def build(self, args):
self.verbose = args.verbose
self.debug_mode = args.debug
# Instantiate necessary modules based on config files
config = configparser.ConfigParser()
config.read(args.sim_config)
config['DEFAULT'] = {'modules': ''}
if 'Simulator' not in config:
raise Exception("Simulator section missing from config file")
sim_settings = config['Simulator']
modules_str = sim_settings['modules']
modules_list = modules_str.split(' ')
for module_name in modules_list:
module_settings = {}
if module_name in config:
module_settings = config[module_name]
self.register(
get_simulator_module(module_name, self, module_settings))
# Build list of modules
for module in self.modules.values():
module.build()
def run(self, trace_file):
self.trace_reader = get_trace_reader(trace_file)
self.trace_reader.build()
if self.debug_mode:
self.debug_interval_cnt = 0
self.debug()
while not self.trace_reader.end_of_trace(
) or not self.event_queue.empty():
# Populate event queue if it is below the threshold number of events
if self.event_queue.size() < Simulator.EVENT_QUEUE_THRESHOLD \
and not self.trace_reader.end_of_trace():
self.populate_event_queue_from_trace()
continue
cur_event = self.event_queue.peek()
trace_event = self.trace_reader.peek_event()
if trace_event and cur_event.timestamp > trace_event.timestamp:
self.populate_event_queue_from_trace()
continue
self.event_queue.pop()
if self.verbose:
print(cur_event)
if self.debug_mode:
self.debug_interval_cnt += 1
if self.debug_interval_cnt == self.debug_interval:
self.debug()
self.debug_interval_cnt = 0
self.broadcast(cur_event)
self.finish()
def populate_event_queue_from_trace(self):
# Fill in event queue from trace
events = self.trace_reader.get_events(
count=Simulator.EVENT_QUEUE_THRESHOLD)
[self.event_queue.push(x) for x in events]
def finish(self):
# Call finish for all modules
for module in self.modules.values():
module.finish()
def subscribe(self, event_type, handler, event_filter=None):
if event_type not in self.event_listeners:
self.event_listeners[event_type] = []
self.event_listeners[event_type].append((event_filter, handler))
def broadcast(self, event):
# Get the set of listeners for the given event type
listeners = self.event_listeners[event.event_type]
for (event_filter, handler) in listeners:
# Send event to each subscribed listener
if not event_filter or event_filter(event):
handler(event)
def register_alarm(self, alarm, handler):
pass
def debug(self):
while True:
command = input("(uamp-sim debug) $ ")
if command:
tokens = command.split(sep=' ')
cmd = tokens[0]
args = tokens[1:]
if cmd == 'quit' or cmd == 'exit' or cmd == 'q':
# TODO(dmanatunga): Handle simulation quitting better
print("Terminating Simulation")
exit(1)
elif cmd == 'interval':
if len(args) == 1:
try:
self.debug_interval = int(args[0])
except ValueError:
print(
"Command Usage Error: interval command expects one numerical value"
)
else:
print(
"Command Usage Error: interval command expects one numerical value"
)
elif cmd == 'verbose':
if len(args) == 0:
self.verbose = True
elif len(args) == 1:
if args[0] == 'on':
self.verbose = True
elif args[0] == 'off':
self.verbose = False
else:
print(
"Command Usage Error: verbose command expects 'on' or 'off' for argument"
)
else:
print(
"Command Usage Error: verbose command expects at most one argument"
)
else:
break
def compute_path(grid,start,goal,cost,heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]]
# The path of the car
path =[['-' for row in range(len(grid[0]))] for col in range(len(grid))]
dist =[]
P =[]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g+h
open_set.put(start, Value(f=f,g=g))
for i in range(0,6):
for j in range(0,5):
if grid[i][j] == 0:
dist = 10000000
x = P(i)
y = P(j
c dfd #cIm just)
open_set.put([i][j], dist)
open_set.insert(goal,0)
while not open_set.empty():
x,y = open_set.pop()
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
if __name__ == "__main__":
path,closed=compute_path(grid, init, goal, cost, heuristic)
for i in range(len(path)):
print(path[i])
print("\nExpanded Nodes")
for node in closed:
print(node)
"""
def shortest_path_to_closest(self, start, end):
class FakePlayer(object):
pass
def heuristic(p):
x, y = p
end_x, end_y = end
return abs(end_x - x) + abs(end_y - y)
frontier = PriorityQueue()
frontier.push((start, 0, None), heuristic(start))
visited = set()
shortest_links = set()
# Explore until no more
while not frontier.empty():
(point, cost, prev) = frontier.pop()
if point not in visited:
visited.add(point)
shortest_links.add((point, cost, prev))
# Create a fake player to move
x, y = point
fake_player = FakePlayer()
fake_player.cards = []
# Add all children
for m in PlayerMovement.all_moves():
fake_player.x = x
fake_player.y = y
if PlayerMovement.move(m, self._board, fake_player):
new_pos = (fake_player.x, fake_player.y)
new_cost = cost + 1
frontier.push((new_pos, new_cost, point), new_cost + heuristic(new_pos))
# Work out closest visited to target
best_link = None
best_heuristic = math.inf
for link in shortest_links:
point, _, _ = link
h = heuristic(point)
if h < best_heuristic:
best_link = link
best_heuristic = h
# Find the reversed path
final_target, _, prev = best_link
target = final_target
rev_path = [target]
while prev is not None:
def match_link(i):
(t, _, p) = i
return t == prev
# Find the prev link
prev_link = list(filter(match_link, list(shortest_links)))
assert(len(prev_link) == 1)
target, _, prev = prev_link[0]
rev_path.append(target)
# Convert the coord-path to a move path
path = list(reversed(rev_path))[1:]
coord_path = path
last_x, last_y = start
move_path = []
while path != []:
next_x, next_y = path[0]
path = path[1:]
if last_x + 1 == next_x:
move_path.append(PlayerMovement.RIGHT)
elif last_x - 1 == next_x:
move_path.append(PlayerMovement.LEFT)
elif last_y + 1 == next_y:
move_path.append(PlayerMovement.DOWN)
elif last_y - 1 == next_y:
move_path.append(PlayerMovement.UP)
last_x = next_x
last_y = next_y
#print("target: {}; coord_path: {}; move_path: {}".format(final_target, coord_path, move_path))
return final_target, move_path # Closest End Coord, Path
def search(self, vobj):
"""The Hybrid State A* search algorithm."""
tic = time.clock()
get_grid_id = self.graph.get_grid_id
frontier = PriorityQueue()
frontier.put(list(self.start), 0)
came_from = {}
cost_so_far = {}
came_from[tuple(self.start)] = None
cost_so_far[get_grid_id(self.start)] = 0
dubins_path = False
num_nodes = 0
while not frontier.empty():
current = frontier.get()
if num_nodes % self.dubins_expansion_constant == 0:
dpath,_ = dubins.path_sample(current, self.goal, self.turning_radius, self.step_size)
if not self.map.is_occupied_discrete(dpath):
# Success. Dubins expansion possible.
self.path_found = True
dubins_path = True
break
if np.linalg.norm(current[0:2] - self.goal[0:2]) < self.eps \
and np.abs(current[2]-self.goal[2]) < np.pi/8:
self.path_found = True
break
for next in self.graph.neighbors(current, vobj.model.est_r_max):
new_cost = cost_so_far[get_grid_id(current)] + \
self.graph.cost(current, next)
if get_grid_id(next) not in cost_so_far or new_cost < cost_so_far[get_grid_id(next)]:
cost_so_far[get_grid_id(next)] = new_cost
priority = new_cost + heuristic(self.goal, next)
frontier.put(list(next), priority)
came_from[tuple(next)] = current
num_nodes += 1
# Reconstruct path
path = [current]
while tuple(current) != tuple(self.start):
current = came_from[tuple(current)]
path.append(current)
if dubins_path:
path = np.array(path[::-1] + dpath)
else:
path = np.array(path[::-2])
print("Hybrid A-star CPU time: %.3f. Nodes expanded: %d" % ( time.clock() - tic, num_nodes))
#print(self.start)
return np.copy(path)
class Simulator(SimulatorBase):
EVENT_QUEUE_THRESHOLD = 100
def __init__(self):
self._sim_modules = {}
self._module_type_map = defaultdict(deque)
self._device_state = DeviceState()
self._event_queue = PriorityQueue()
self._current_time = None
self._warmup_period = None
self._event_listeners = defaultdict(deque)
self._trace_reader = None
self._trace_executed = False
self._verbose = False
self._debug_mode = False
self._debug_interval = 1
self._debug_interval_cnt = 0
def has_module_instance(self, name):
return name in self._sim_modules
def get_module_instance(self, name):
return self._sim_modules[name]
def get_module_for_type(self, module_type):
if module_type in self._module_type_map:
return self._module_type_map[module_type.value][0]
else:
return None
def register(self, sim_module, override=False):
if not isinstance(sim_module, SimModule):
raise TypeError("Expected SimModule object")
if sim_module.get_name() in self._sim_modules:
raise Exception("Module %s already exists" % sim_module.get_name())
self._sim_modules[sim_module.get_name()] = sim_module
if override:
self._module_type_map[sim_module.get_type().value].appendleft(sim_module)
else:
self._module_type_map[sim_module.get_type().value].append(sim_module)
def build(self, args):
self._verbose = args.verbose
self._debug_mode = args.debug
# Instantiate necessary modules based on config files
config = configparser.ConfigParser()
config.read(args.sim_config)
config['DEFAULT'] = {'modules': '', 'warmup_period': ''}
if 'Simulator' not in config:
raise Exception("Simulator section missing from config file")
sim_settings = config['Simulator']
# Identify the set of modules to include
modules_str = sim_settings['modules']
if modules_str:
modules_list = modules_str.split(' ')
else:
modules_list = []
self._warmup_period = \
self.__parse_warmup_setting(sim_settings['warmup_period'])
# Setup the trace file reader and initial simulator time
self._trace_reader = get_trace_reader(args.trace)
self._trace_reader.build()
self._trace_executed = False
self._current_time = self._trace_reader.get_start_time()
for module_name in modules_list:
module_settings = {}
if module_name in config:
module_settings = config[module_name]
self.register(get_simulator_module(module_name, self, module_settings))
# Build list of modules
for sim_module in self._sim_modules.values():
sim_module.build()
def run(self):
# Check if we need to enter debug mode immediately
if self._debug_mode:
self._debug_interval_cnt = 0
self.__debug()
# Add alarm event for the warmup period
warmup_finish_alarm = SimAlarm(
timestamp=self._trace_reader.get_start_time() + self._warmup_period,
handler=self.__enable_stats_collection,
name='Warmup Period Alarm')
self._event_queue.push(warmup_finish_alarm,
(warmup_finish_alarm.timestamp, Priority.SIMULATOR))
while not self._trace_reader.end_of_trace() \
or not self._event_queue.empty():
# Populate event queue if it is below the threshold number of events
if self._event_queue.size() < Simulator.EVENT_QUEUE_THRESHOLD \
and not self._trace_reader.end_of_trace():
self.__populate_event_queue_from_trace()
continue
# Look at next event to execute
cur_event = self._event_queue.peek()
# Look at next event from trace file
trace_event = self._trace_reader.peek_event()
# If trace event is supposed to occur before current
# event, than we should populate event queue with more
# events from the trace file
if trace_event and cur_event.timestamp > trace_event.timestamp:
self.__populate_event_queue_from_trace()
continue
self._event_queue.pop()
# Set current time of simulator
self._current_time = cur_event.timestamp
if self._verbose:
print(cur_event)
if self._debug_mode:
self._debug_interval_cnt += 1
if self._debug_interval_cnt == self._debug_interval:
self.__debug()
self._debug_interval_cnt = 0
self.__execute_event(cur_event)
self.__finish()
def subscribe(self, event_type, handler, event_filter=None):
if event_type not in self._event_listeners:
self._event_listeners[event_type] = []
self._event_listeners[event_type].append((event_filter, handler))
def broadcast(self, event):
if event.timestamp:
if event.timestamp != self._current_time:
raise Exception("Broadcasting event with invalid timestamp.")
else:
event.timestamp = self._current_time
# Get the set of listeners for the given event type
listeners = self._event_listeners[event.event_type]
for (event_filter, handler) in listeners:
# Send event to each subscribed listener
if not event_filter or event_filter(event):
handler(event)
def register_alarm(self, alarm):
self._event_queue.push(alarm, (alarm.timestamp, Priority.ALARM))
def get_current_time(self):
return self._current_time
def get_device_state(self):
return self._device_state
def __parse_warmup_setting(self, setting_value):
if setting_value:
if setting_value.endswith('h'):
num_hours = int(setting_value[:-1])
return datetime.timedelta(hours=num_hours)
raise Exception("Invalid warmup period setting format")
else:
return datetime.timedelta()
def __enable_stats_collection(self):
for sim_module in self._sim_modules.values():
sim_module.enable_stats_collection()
def __disable_stats_collection(self):
for sim_module in self._sim_modules.values():
sim_module.disable_stats_collection()
"""
Private method that handles execution of
an event object.
"""
def __execute_event(self, event):
if event.event_type == EventType.SIM_DEBUG:
self.__debug()
elif event.event_type == EventType.SIM_ALARM:
if not self._trace_executed:
event.fire()
if event.is_repeating():
self._event_queue.push(event, (event.timestamp, Priority.ALARM))
elif event.event_type == EventType.TRACE_END:
self._trace_executed = True
else:
self.broadcast(event)
def __populate_event_queue_from_trace(self):
# Fill in event queue from trace
events = self._trace_reader.get_events(count=Simulator.EVENT_QUEUE_THRESHOLD)
for x in events:
self._event_queue.push(x, (x.timestamp, Priority.TRACE))
def __finish(self):
output_file = sys.stdout
# Print status from all modules
for sim_module in self._sim_modules.values():
header = "======== %s Stats ========\n" % sim_module.get_name()
footer = "=" * (len(header) - 1) + '\n'
output_file.write(header)
sim_module.print_stats(output_file)
output_file.write(footer)
# Call finish for all modules
for sim_module in self._sim_modules.values():
sim_module.finish()
def __debug(self):
while True:
command = input("(uamp-sim debug) $ ")
if command:
tokens = command.split(sep=' ')
cmd = tokens[0]
args = tokens[1:]
if cmd == 'quit' or cmd == 'exit' or cmd == 'q':
# TODO(dmanatunga): Handle simulation quitting better
print("Terminating Simulation")
exit(1)
elif cmd == 'interval':
if len(args) == 1:
try:
self._debug_interval = int(args[0])
except ValueError:
print("Command Usage Error: interval command expects one numerical value")
else:
print("Command Usage Error: interval command expects one numerical value")
elif cmd == 'verbose':
if len(args) == 0:
self._verbose = True
elif len(args) == 1:
if args[0] == 'on':
self._verbose = True
elif args[0] == 'off':
self._verbose = False
else:
print("Command Usage Error: verbose command expects 'on' or 'off' for argument")
else:
print("Command Usage Error: verbose command expects at most one argument")
else:
break
def search(self, vobj):
"""The Hybrid State A* search algorithm."""
tic = time.clock()
get_grid_id = self.graph.get_grid_id
frontier = PriorityQueue()
frontier.put(list(self.start), 0)
came_from = {}
cost_so_far = {}
came_from[tuple(self.start)] = None
cost_so_far[get_grid_id(self.start)] = 0
dubins_path = False
num_nodes = 0
while not frontier.empty():
current = frontier.get()
if num_nodes % self.dubins_expansion_constant == 0:
dpath,_ = dubins.path_sample(current, self.goal, self.turning_radius, self.step_size)
if not self.map.is_occupied_discrete(dpath):
# Success. Dubins expansion possible.
self.path_found = True
dubins_path = True
break
if np.linalg.norm(current[0:2] - self.goal[0:2]) < self.eps \
and np.abs(current[2]-self.goal[2]) < np.pi/8:
self.path_found = True
break
for next in self.graph.neighbors(current, vobj.model.est_r_max):
new_cost = cost_so_far[get_grid_id(current)] + \
self.graph.cost(current, next)
if get_grid_id(next) not in cost_so_far or new_cost < cost_so_far[get_grid_id(next)]:
cost_so_far[get_grid_id(next)] = new_cost
priority = new_cost + heuristic(self.goal, next)
frontier.put(list(next), priority)
came_from[tuple(next)] = current
num_nodes += 1
# Reconstruct path
path = [current]
while tuple(current) != tuple(self.start):
current = came_from[tuple(current)]
path.append(current)
if dubins_path:
path = np.array(path[::-1] + dpath)
else:
path = np.array(path[::-2])
print "Hybrid A-star CPU time: %.3f. Nodes expanded: %d" % ( time.clock() - tic, num_nodes)
#print self.start
return np.copy(path)
import matplotlib.pyplot as plt
G = nx.karate_club_graph()
for edge in G.edges():
G[edge[0]][edge[1]]['weight'] = random.randint(1, 10)
source = 2
goal = 3
ref_nodes = PriorityQueue()
ref_nodes.put(source, 0)
dist_so_far = {}
came_from = {}
dist_so_far[source] = 0
while not ref_nodes.empty():
current = ref_nodes.get()
if current == goal:
break
for next_node in G.neighbors(current):
new_dist = dist_so_far[current] + G[current][next_node]['weight']
if next_node not in dist_so_far or new_dist < dist_so_far[next_node]:
dist_so_far[next_node] = new_dist
ref_nodes.put(next_node, new_dist)
came_from[next_node] = current
print(came_from)
print(dist_so_far)
