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 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))]
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))
# 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))]
while open_set:
node = open_set.pop()
closed_set.add(node[0])
if node[0] == goal:
break
#finding child of node
#write function for finding child node
#setting the cost values for neighboring nodes
neighbors = get_neighbors(node[0])
print(neighbors)
for i in neighbors:
x = i[0]
y = i[1]
theta = i[2]
h = heuristic[x][y]
g = node[1].g + costfunction(node[0],i)
f = g+h
if i not in open_set or i not in closed_set:
open_set.put(i,Value(f,g))
elif i in open_set and f < open_set.get(i).f:
open_set.put(i,Value(f,g))
return path, closed_set
class TransactionPool:
"""Transaction pool for a miner"""
def __init__(self, env, identifier, neighbourList, nodes, params):
self.env = env
self.identifier = identifier
self.neighbourList = neighbourList
self.params = params
self.nodes = nodes
self.transactionQueue = PriorityQueue()
self.prevTransactions = []
def getTransaction(self, transactionCount):
"""Returns transactionCount number of Transactions. Returns
top transactions based on miner reward"""
return self.transactionQueue.get(transactionCount)
def popTransaction(self, transactionCount):
"""Remove transactions from transaction pool. Called when transactions
are added by a received block or a block is mined."""
poppedTransactions = self.transactionQueue.pop(transactionCount)
self.prevTransactions.append(poppedTransactions)
def putTransaction(self, transaction, sourceLocation):
"""Add received transaction to the transaction pool and broadcast further"""
destLocation = self.nodes[self.identifier].location
delay = getTransmissionDelay(sourceLocation, destLocation)
yield self.env.timeout(delay)
if (
not self.transactionQueue.isPresent(transaction)
and transaction not in self.prevTransactions
):
self.transactionQueue.insert(transaction)
broadcast(
self.env,
transaction,
"Transaction",
self.identifier,
self.neighbourList,
self.params,
nodes=self.nodes,
)
if self.params["verbose"] == "vv":
print(
"%7.4f : %s accepted by %s"
% (self.env.now, transaction.identifier, self.identifier)
)
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)
def shortest_path(self, start_, end_, heuristic, undesired):
assert self.is_node(start_) and self.is_node(end_)
self.graph.reset()
start_ = self.graph.get_node(start_)
end_ = self.graph.get_node(end_)
pq = PriorityQueue()
# A* shortest path
start_.cost = 0
start_.visited = True
pq.put(start_, 0.0)
while not pq.is_empty():
current = pq.get()
if current.position == end_.position:
break
for (neighbor, weight) in current.neighbors:
if current.position == undesired or neighbor.position == undesired:
weight = 10000 # Mask out to avoid crossing
g = current.cost + weight
if not neighbor.visited or g <= neighbor.cost:
neighbor.cost = g
neighbor.parent = current
f = g + heuristic(neighbor.position, end_.position)
pq.put(neighbor, f)
neighbor.visited = True
# Unrol shortest path
path = []
node = end_
while (node != None):
path.append(node.position)
if end_ != (0, 0) and node.cost >= 5000: return None
if node == start_: break
node = node.parent
path.reverse()
if node != start_:
return None
return path
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)
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))]
x = start[0]
y = start[1]
theta = start[2]
g_value = 0
h_value = heuristic[x][y]
f = g_value + h_value
parent_cost = [[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))]]
open_set.put(start, Value(f=f, g=g_value))
# Implement of A* algorithm:
while open_set:
node1, cost_new = open_set.pop()
print('The current node is:', node1)
print('---------------------------------')
if node1 == goal:
closed_set.add(node1)
while node1 != start:
node0 = parent[node1[2]][node1[0]][node1[1]]
action = rotate(node0, node1)
node1 = node0
path[node0[0]][node0[1]] = action_name[action + 1]
return path, closed_set
children = children_nodes(grid, node1)
print('The children present are:', children)
print(
'=============================================================================='
)
closed_set.add(node1)
for child in children:
direction = rotate(node1, child)
cost_child, child_new = heuristics_value(cost_new, direction,
child)
if child not in open_set or closed_set:
open_set.put(child, Value(f=cost_child, g=child_new))
if child in open_set:
cost_need = open_set.get(child)
cost_f = cost_need.f
if cost_child < cost_f:
open_set.put(child, Value(f=cost_child, g=child_new))
if parent_cost[child[2]][child[0]][child[1]] > cost_child:
parent_cost[child[2]][child[0]][child[1]] = cost_child
parent[child[2]][child[0]][
child[1]] = node1[0], node1[1], node1[2]
return path, closed_set
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))]]
#print(parent)
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
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))
# 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))]
while open_set:
node = open_set.pop()
closed_set.add(node[0])
if (node[0] == goal):
break
# Finding child for each node.
neighbours = find_child(node[0])
for neighbour in neighbours:
#print(neighbour)
#print(node[0])
g = node[1].g + cost_neigh(neighbour, node[0])
x = neighbour[0]
y = neighbour[1]
theta = neighbour[2]
h = heuristic[x][y]
f = g + h
if (neighbour not in open_set or neighbour not in closed_set):
open_set.put(neighbour, Value(f, g))
a = parent[theta][x][y]
if (a == ' ' or f < a[1].g + cost_neigh(neighbour, a[0]) + h):
parent[theta][x][y] = node
elif (neighbour in open_set and f < open_set.get(neighbour).f):
open_set.put(neighbour, Value(f, g))
if (a == ' ' or f < a[1].g + cost_neigh(neighbour, a[0]) + h):
parent[theta][x][y] = node
g = goal
d = []
while (g != start):
#print(g)
d.append(g)
g = parent[g[2]][g[0]][g[1]][0]
d.append(g)
d.reverse()
#print(d)
direction_list = []
for i in range(0, len(d) - 1):
direction = get_direction(d[i + 1], d[i])
direction_list.append(direction)
#print(direction_list);
direction_list.append("*")
for i in range(0, len(d)):
x = d[i][0]
y = d[i][1]
path[x][y] = direction_list[i]
return path, closed_set
class NodeFull(NodeBase):
def __init__(self, config):
"""
Implements full node that not only tracks other nodes, but also
maintains full blockchain, accepts transactions and mines new blocks.
"""
super(NodeFull, self).__init__(config)
self._hash_prev = None
self._blockchain = set()
# Producer-consumer queues for exchanging data with the mining thread
self._transaction_queue = PriorityQueue(f_priority=favor_higher_fees)
self._mining = config.mining
# Launch mining and block and transaction discovery
if self._mining:
Thread(target=self.mine, name="mine").start()
# Launch transaction generating and sharing
if config.gen_transactions:
Thread(target=self.generate_transactions).start()
Thread(target=self.discover_blocks).start()
Thread(target=self.share_blocks).start()
Thread(target=self.discover_transactions).start()
Thread(target=self.share_transactions).start()
def add_genesis_block(self):
"""
Assembles genesis block, the first block in the blockchain.
It is hardcoded and the same for every mining node.
"""
coinbase_transaction = self.create_coinbase_transaction(
dest_key=constants.GENESIS_BLOCK_DEST_KEY)
transactions = [coinbase_transaction]
genesis_block = blockchain.Block(
hash_prev=constants.GENESIS_BLOCK_HASH_PREV,
difficulty=constants.GENESIS_BLOCK_DIFFICULTY,
hash=constants.GENESIS_BLOCK_HASH,
merkle_root=constants.GENESIS_BLOCK_MERKLE_ROOT,
nonce=constants.GENESIS_BLOCK_NONCE,
extra_nonce=constants.GENESIS_BLOCK_EXTRA_NONCE,
transactions=transactions)
self._hash_prev = genesis_block.hash
self.maybe_add_block(genesis_block)
def maybe_add_block(self, block):
for transaction in block.transactions:
self._transaction_queue.remove(transaction)
self._blockchain.add(block)
def maybe_add_transaction(self, transaction):
transactions = [
tx for block in list(self._blockchain)
for tx in list(block.transactions)
]
# Skip adding transaction that is already in the blockchain
if transaction in transactions:
return
# Skip adding transaction that is already queued
if transaction in self._transaction_queue.items:
return
self._transaction_queue.put(transaction)
def generate_transactions(self):
"""
Generates random transactions
"""
transaction_generator = blockchain.TransactionGeneratorFake(
blocks=self._blockchain,
peers=self._known_peers,
ecdsa=self._ecdsa)
while True:
if len(self._transaction_queue.items) < 10:
console.debug("generating transaction")
transaction = transaction_generator.generate()
self.maybe_add_transaction(transaction)
else:
time.sleep(3)
def create_coinbase_transaction(self, dest_key=None):
"""
Creates a coinbase transaction. This transaction is included as the
first transaction of the block and is creating a fixed amount of coins
"""
coinbase_output = blockchain.TransactionOutput(
amount=constants.BLOCK_COINBASE_AMOUNT,
key=dest_key or self._ecdsa.public_key)
coinbase_tx = blockchain.Transaction(
inputs=[],
outputs=[coinbase_output],
)
return coinbase_tx
def mine(self):
"""
Mines new blocks
"""
self.add_genesis_block()
while self._mining:
coinbase_transaction = self.create_coinbase_transaction()
transactions = [coinbase_transaction]
for _ in range(constants.BLOCK_MAX_TRANSACTIONS - 1):
# transaction = self._transaction_generator.generate()
transaction = self._transaction_queue.get()
transactions.append(transaction)
block = blockchain.Block(
hash_prev=self._hash_prev,
difficulty=self._config.difficulty,
transactions=transactions,
)
console.info("Mining a new block:" + " " * 32 +
"\n{:}\n".format(block.summary))
found = False
while not found:
found = self.mine_one_iteration(block)
# Throttle mining to reduce CPU load (for the demo)
time.sleep(self._config.mining_throttle_ms / 1000)
def mine_one_iteration(self, block: blockchain.Block):
"""
Perform one iteration of block mining (increments the nonce once)
"""
found, nonce, curr_hash = block.mine_one()
sys.stdout.write("Nonce: {:010d} | Hash : {:}\r".format(
nonce, bin_str(bytes_to_int(curr_hash), pad=service.hash_f.bits)))
if found:
console.info("Found a new block:" + " " * 32 +
"\n{:}\n".format(block.details))
self.maybe_add_block(block)
self._hash_prev = block.hash
return found
def discover_transactions(self):
if self._config.transaction_discovery_interval >= 0:
while True:
known_peers = set(self._known_peers)
for peer in known_peers:
transactions = peer.get_transactions()
for transaction in transactions:
self.maybe_add_transaction(transaction)
time.sleep(self._config.transaction_discovery_interval)
def share_transactions(self):
if self._config.transaction_sharing_interval >= 0:
while True:
known_peers = set(self._known_peers)
transactions = self._transaction_queue.items
for peer in known_peers:
peer.send_transactions(transactions)
time.sleep(self._config.transaction_sharing_interval)
def discover_blocks(self):
if self._config.block_discovery_interval >= 0:
while True:
known_peers = set(self._known_peers)
for peer in known_peers:
blocks = peer.get_blocks()
for block in blocks:
self.maybe_add_block(block)
time.sleep(self._config.block_discovery_interval)
def share_blocks(self):
if self._config.block_sharing_interval >= 0:
while True:
known_peers = set(self._known_peers)
blocks = list(self._blockchain)
for peer in known_peers:
peer.send_blocks(blocks)
time.sleep(self._config.block_sharing_interval)
def get_transactions(self, _, __):
"""
RPC server handler triggered on get_transactions RPC call
"""
transactions = list(self._transaction_queue.items)
for transaction in transactions:
yield transaction.to_proto()
def send_transactions(self, transactions: Iterable, _):
"""
RPC server handler triggered on send_transactions RPC call
"""
for transaction in transactions:
transaction = blockchain.Transaction.from_proto(transaction)
if transaction is not None:
self.maybe_add_transaction(transaction)
return service.messages.Empty()
def get_blocks(self, _, __):
"""
RPC server handler triggered on get_blocks RPC call
"""
blocks = list(self._blockchain)
for block in blocks:
yield block.to_proto()
def send_blocks(self, blocks: Iterable, _):
"""
RPC server handler triggered on send_blocks RPC call
"""
for block in blocks:
block = blockchain.Block.from_proto(block)
if block is not None:
self.maybe_add_block(block)
return service.messages.Empty()
def dijkstra(grid, start, goal, cost): #dijkstra algorithm
closed_set = OrderedSet()
open_set = PriorityQueue(order=min, f=lambda v: v.f)
l = []
l.append(goal[:2])
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
dist = [[float('inf') for row in range(len(grid[0]))]
for col in range(len(grid))]
parent = [['NULL' for row in range(len(grid[0]))]
for col in range(len(grid))]
open_set.put(start[:2],
Value(f=dist[start[0]][start[1]], g=(goal[2], 'null')))
path[goal[0]][goal[1]] = '*'
for x in range(len(grid)):
for y in range(len(grid[0])):
if ((x, y) != (goal[0], goal[1])):
dist[x][y] = float('inf')
parent[x][y] = 'NULL'
open_set.put((x, y), Value(f=dist[x][y], g=('NULL', 'NULL')))
open_set.put(goal[:2], Value(f=0, g=(goal[2], 'null')))
dist[goal[0]][goal[1]] = 0
while (len(open_set) != 0):
curr_node = open_set.pop()
closed_set.add(curr_node)
if (curr_node[0][:2] != goal[:2]):
path[l[-1][0]][l[-1][1]] = curr_node[1].g[1]
l.append(curr_node[0][:2])
x_current = curr_node[0][0] #row of current node
y_current = curr_node[0][1] #col of current node
o_current = curr_node[1].g[0] #orientation of current node
if (curr_node[0] == start[:2]):
print("get path! ")
break
for i_act, act in enumerate(action):
o_next = (o_current - act) % 4 #orientation of next node
x_next = x_current - forward[o_next][0] #row of next node
y_next = y_current - forward[o_next][1] #col of next node
n_act = action_name[
i_act] #action to get this next node, this cause the one step delay in the display
if (x_next >= 0 and x_next < len(grid) and y_next >= 0
and y_next < len(grid[0])
): #filter the available child (map boundery and barrier)
if (grid[x_next][y_next] == 0):
F_cost = cost[i_act] + dist[x_current][
y_current] #calculate f(n) = g(n)
F_raw = dist[x_next][y_next]
if (((x_next, y_next, o_next) not in closed_set)
and F_cost < F_raw): #child not in closed_set
open_set.get((x_next, y_next)).f = F_cost
open_set.get((x_next, y_next)).g = (o_next, n_act)
dist[x_next][y_next] = F_cost
parent[x_next][y_next] = (x_current, y_current,
o_current)
if (curr_node[0][:2] != start[:2]): print("fail to find the path!")
return path, closed_set, parent
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))]]
# creating a node cost list similar in structure to parent list with inital value of 100
# used to determine whether the parent list needs to be updated for a particular child
# if the new cost to reach a child is less than the previous calculated cost
# then parent list and node_cost list will be updated with new found parent node and cost
node_cost = [[[100 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[100 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[100 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[100 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))]
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))
# your code: implement A*
while open_set: #code runs in loop as long as there is a node in open_set
current_node, current_value = open_set.pop(
) #current node is minimum from open_set
if current_node == goal: #to stop the code once goal is reached
closed_set.add(current_node) #adding goal to closed_set
while current_node != start: #to back track, from end to start, loop runs till we backtrack to start node
parent_node = parent[current_node[2]][current_node[0]][
current_node[
1]] #accessing parent list to find the parent node of current node
action = move_action(
parent_node, current_node
) #to determine what action was required to move from parent node to current node
path[parent_node[0]][parent_node[1]] = action_name[
action +
1] #updating path list with action required to move from parent to child node
current_node = parent_node #now parent node becomes current node before path identification code runs again
pprint.pprint(path)
return path, closed_set
closed_set.add(current_node) #adding current node to closed_set
neighbours = find_neighbour(
grid, current_node
) #to determine the list of feasible children for current node
for neighbour in neighbours: #running through all feasible childs
actions = move_action(
current_node, neighbour
) #determining action required to move from current node to child node
total_cost_neighbour, step_cost_neighbour = find_value(
actions, current_value, neighbour
) #calculating total & path cost for moving to child node
if neighbour not in open_set or closed_set: # if child is not present in open or closed set then add to open_set
open_set.put(
neighbour,
Value(f=total_cost_neighbour, g=step_cost_neighbour))
# incase child is present in open set then new & old value are compared and if new value is low then it is updated
elif neighbour in open_set:
f_prev = open_set.get(
neighbour) #accessing old value of child from open_set
f_prev_f = f_prev.f
if total_cost_neighbour < f_prev_f: #comparing old and new values
open_set.put(
neighbour,
Value(f=total_cost_neighbour, g=step_cost_neighbour)
) #updating open set if new value is less than old value
#if cost to travel to child is found to be lesser than previously calculated cost for the child then its parent and cost is updated
if node_cost[neighbour[2]][neighbour[0]][
neighbour[1]] > total_cost_neighbour:
parent[neighbour[2]][neighbour[0]][neighbour[
1]] = current_node[0], current_node[1], current_node[
2] #updating parent node
node_cost[neighbour[2]][neighbour[0]][
neighbour[1]] = total_cost_neighbour #updating cost
# 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
def search(self, start, goal):
"""
Perform the search. If given a new graph, replace the current one with it, and start fresh.
Also, if specified, start fresh with the current graph. This is non-recursive and can therefore
run infinitely until memory is exhausted.
:param start: The node to start from
:param goal: The node to go to
:return:
"""
# start with a Priority Queue to hold the nodes to visit.
# A priority queue means we'll visit the most promising nodes first
# because they'll get higher priority
to_visit = PriorityQueue()
# But for now, we need to start by only knowing where we're at. It has lowest
# priority because when you're trying to catch someone, (in this case) it's
# best to keep moving
to_visit.put(start, 0)
# Yay hash tables! This will store the path we took while visiting nodes. Sorta kinda
# like a linked list, but not (almost)
visited = dict()
# Woo hash tables! Here we'll store hashes of the nodes (tuples) we visit
# and their costs
costs = dict()
# We've visited where we're starting from. It's nice and all, but the scenery is
# kinda bland. Oh and we didn't arrive here from anywhere, so the previous node
# is nothing.
visited[start] = None
# It didn't cost us anything to get here, which is nice since I'm pretty broke
costs[start] = 0
# While we have things in the queue to visit, check out the neighbors
# and see if they're worth visiting. I mean, I'm sure they're lovely people
# and all, but if they can't get us to our destination, then we don't need to
# waste our time. It's the capitalist way! Also, they're weird and have electric
# lawnmowers.
while not to_visit.is_empty:
# Travel to the next best node
current = to_visit.get()
# Hey! We got to where we needed to go!
if current == goal:
break
# Here's where we figure out if we're gonna be neighborly and visit nodes
# surrounding us. Reasons for not visiting our neighbors:
# 1) They're a wall. It'd look weird to visit a wall
# 2) It'd be more costly to visit them than others.
# 3) They support Trump
# 4) They use electric lawnmowers
# 5) They're clingy
# 6) They always ask for a cup of sugar but when you try to reciprocate,
# they're always suddenly out
# 7) They listen to Insane Clown Posse
for next in self.board.nodes_to_visit(current):
new_cost = costs[current] + self.board.cost(next)
# See item two in the above list
if next not in costs or new_cost < costs.get(next, Infinity):
# Oh hey, it's cheaper to visit them than not. Cool!
# Record the cost of visiting the neighbor
costs[next] = new_cost
# Figure out where they stand on your priorities list
# and put them in the Queue. Hermes would be proud
to_visit.put(next, new_cost + AStar.heuristic(goal, next))
# Mark down how you got to them
visited[next] = current
# Okay, we've figured out a path. It's... somewhere in here. Hrm...
return visited, costs
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))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
# your code: implement A*
open_set.put(start, Value(f=f, g=g))
while open_set.__len__() != 0:
node = open_set.pop()
g = node[1].g
x = node[0][0]
y = node[0][1]
theta = node[0][2]
#if reach the goal, break
if (node[0] == goal):
closed_set.add(node[0])
break
closed_set.add(node[0])
for idx, act in enumerate(action):
new_theta = (theta + act) % 4
#use orientation to determine the position
new_x = x + forward[new_theta][0]
new_y = y + forward[new_theta][1]
#to make sure it won't break the node is naviable
if (new_x < 0 or new_x > 4 or new_y < 0 or new_y > 5
or grid[new_x][new_y] == 1):
continue
new_g = g + cost[idx]
new_h = heuristic[new_x][new_y]
new_f = new_g + new_h
child = [(new_x, new_y, new_theta), Value(f=new_f, g=new_g)]
if (child[0] not in closed_set and child[0] not in open_set):
open_set.put(child[0], child[1])
parent[new_theta][new_x][new_y] = (action_name[idx], node[0])
#if the cost decreased, add it to the openlist
elif (open_set.has(child[0]) and open_set.get(child[0]).g > new_g):
open_set.put(child[0], child[1])
parent[new_theta][new_x][new_y] = (action_name[idx], node[0])
#recording the path in parent
#reach the goal
path[x][y] = '*'
#find out the path by recalling step by step
while (x, y, theta) != start:
pre_step = parent[theta][x][y]
x = pre_step[1][0]
y = pre_step[1][1]
theta = pre_step[1][2]
path[x][y] = pre_step[0]
# 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
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))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
children = []
# print(start)
open_set.put(start, Value(f=f, g=g))
while open_set:
node, new_cost = open_set.pop()
print('The current node is:', node)
print('==============================')
if node == goal:
return path, closed_set
closed_set.add(node)
row_grid = len(grid) - 1 #4 Boundry of the grids
column_grid = (len(grid[0])) - 1 #5
x1 = node[0] #Parrent x co-ordinate
y1 = node[1] # Parrent y co-ordinate
theta1 = node[2] # Parrent orientation
f1 = new_cost.f #Parent total cost
h = heuristic[x1][y1] #Heuristic of parrent
g1 = new_cost.g #Path cost of parrent
neighbor = []
f12 = []
h12 = []
g12 = []
for i in range(len(action)):
temp_dir = collections.deque(
direction
) # creating a deque tuple # 0 N :::: 1 west 2 south :::: east 3
index = theta1 # orientation of the Parent
child_index = action[i] # possible index of the child
n = -1 * child_index
temp_dir.rotate(n)
child_theta = temp_dir[
theta1] # using the index of the parent to find out the current orientation of the child
""" calculating the distances of children"""
x_pos = node[0] + forward[child_theta][
0] # this gives the position of x,y of the child
y_pos = node[1] + forward[child_theta][1]
pos = (x_pos, y_pos, child_theta)
g1 = []
if (pos[0] > row_grid or pos[1] > (column_grid) or pos[1] < 0
or pos[0] < 0):
continue
elif (grid[pos[0]][pos[1]] == 1):
continue
else:
neighbor.append(pos)
h1 = heuristic[x_pos][
y_pos] # heuristic of neighbor that is dist from the goal
g1 = new_cost.g + cost[i] # path function
print(new_cost.g)
print("---------------------")
print(cost[i])
f1 = g1 + h1 # total cost
f12 = np.append(f12, f1)
g12 = np.append(g12, g1)
h12 = np.append(h12, h1)
gmin = min(g12)
posi = np.argmin(g12)
c = 0
for neigh in neighbor: #Condition for child which is closest to the goal
if neigh not in open_set or neigh not in closed_set:
open_set.put(neigh, Value(f=f12[c], g=g12[c]))
# print(open_set.get(neigh).g)
print(neigh)
print(open_set.get(neigh).g)
elif neigh in open_set and gmin < open_set.get(neigh).g:
open_set.put(neigh, Value(f=f12[c], g=g12[c]))
# print("ggggggggg")
c = c + 1
c = 0
return path, closed_set
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)
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)
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))]
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))
# 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))]
path[goal[0]][goal[1]] = '*'
l = []
l.append(start)
while (len(open_set) != 0):
curr_node = open_set.pop()
if (curr_node[0][:2] != start[:2]):
path[l[-1][0]][l[-1][1]] = curr_node[1].g
l.append(curr_node[0])
if (curr_node[0] == goal):
print("get path!")
closed_set.add(curr_node)
break
closed_set.add(curr_node)
x_current = curr_node[0][0] #row of current node
y_current = curr_node[0][1] #col of current node
o_current = curr_node[0][2] #orientation of current node
for i_act, act in enumerate(action):
o_next = (o_current + act) % 4 #orientation of next node
x_next = x_current + forward[o_next][0] #row of next node
y_next = y_current + forward[o_next][1] #col of next node
n_act = action_name[
i_act] #action to get this next node, this cause the one step delay in the display
if (x_next >= 0 and x_next < len(grid) and y_next >= 0
and y_next < len(grid[0])
): #filter the available child (map boundery and barrier)
if (grid[x_next][y_next] == 0):
F_cost = cost[i_act] + heuristic[x_next][
y_next] # abs(x_next - goal[0]) + abs(y_next - goal[1]) #calculate the f(n) = g(n) + h(n)
if ((not open_set.has((x_next, y_next, o_next)))
or (not closed_set.has((x_next, y_next, o_next)))):
open_set.put((x_next, y_next, o_next),
Value(f=F_cost, g=n_act))
#parent[x_next][y_next] = (x_next, y_next, o_next)
elif (open_set.has((x_next, y_next, o_next))
and F_cost < open_set.get(
(x_next, y_next, o_next)).f):
open_set.get((x_next, y_next, o_next)).f = F_cost
open_set.get((x_next, y_next, o_next)).g = n_act
if (curr_node[0] != goal): print("fail to find the path!")
return path, closed_set
