def aStar(start, goal, neighbor_func, distance_func, heuristic_func):
"""Returns a sequence of nodes that optmizes for the least cost
from the start node to the goal.
Let's describe the data that we pass to this function:
start: the start of the search.
goal: the goal of the search.
neighbor_func: a function that, given a state, returns a list of
neighboring states.
distance_func: a function that takes two nodes, and returns
the distance between them.
heuristic_func: a function that takes two nodes, and returns
the heuristic distance between them.
Each state mush be hashable --- each state must support the
hash() function.
"""
pqueue = PriorityQueue()
g_costs = {start : 1}
parents = {start : start}
pqueue.push(heuristic_func(start, goal), start)
while not pqueue.isEmpty():
next_cost, next_node = pqueue.pop()
g_costs[next_node] = g_costs[parents[next_node]] \
+ distance_func(next_node, parents[next_node])
if next_node == goal: break
children = neighbor_func(next_node)
for child in children:
updateChild(goal, distance_func, heuristic_func,
child, next_node, parents, g_costs, pqueue)
return getPathToGoal(start, goal, parents)
def run(self):
pq = PriorityQueue()
for q in self.queues:
t = q.reset()
pq.push(t, q)
while not pq.empty():
t = max(t, pq.key())
q = pq.value()
pq.pop()
next_t = q.process(t)
if next_t >= 0:
pq.push(next_t, q)
return t
def a_search(graph, start, goals):
my_heap = PriorityQueue(
) #not really a heap just easier to call it that than queue
my_heap.push(start, 0)
pathway = []
cost_so_far = {}
came_from = {}
cost_so_far[start] = 0
came_from[start] = None
the_sum = 0
ordered_goals = []
while not my_heap.empty():
current = my_heap.pop()
#print("here")
if current in goals:
ordered_goals.append(current)
goals.remove(current)
for i in graph:
if i.parent:
the_sum += 1
while current.parent:
pathway.append(current)
x = current.parent
current.parent = None
current = x
if len(goals) == 0:
break
for i in current.edges:
new_score = current.gscore + 1 #increment g(n)
if i not in cost_so_far or new_score < i.gscore:
cost_so_far[i] = new_score
i.gscore = new_score
i.hscore = findMST(i, goals)
#print(i.hscore, current)
i.fscore = i.gscore + i.hscore
my_heap.push(i, i.fscore)
if not i.parent:
i.parent = current
return pathway, the_sum, ordered_goals
class TestPriorityQueue(unittest.TestCase):
def setUp(self):
self.pq = PriorityQueue(":memory:")
def test_empty(self):
result = self.pq.is_empty()
self.assertTrue(result)
def test_order(self):
self.assertEqual("hello", self.pq.push(10, "hello"))
self.pq.push(1, "foo")
self.pq.push(30, "bar")
self.pq.push(5, "baz")
self.assertEqual("bar", self.pq.pop())
self.assertEqual("hello", self.pq.pop())
self.assertEqual("baz", self.pq.pop())
self.assertEqual("foo", self.pq.pop())
class TestPriorityQueue(unittest.TestCase):
def setUp(self):
self.pq = PriorityQueue(":memory:")
def test_empty(self):
result = self.pq.is_empty()
self.assertTrue(result)
def test_order(self):
self.assertEqual("hello", self.pq.push(10, "hello"))
self.pq.push(1, "foo")
self.pq.push(30, "bar")
self.pq.push(5, "baz")
self.assertEqual("bar", self.pq.pop())
self.assertEqual("hello", self.pq.pop())
self.assertEqual("baz", self.pq.pop())
self.assertEqual("foo", self.pq.pop())
class Node():
def __init__(self, node_id):
self.node_id = node_id
self.timestamp = 0
self.nodeudp = NodeUDP(node_id)
self.thread = threading.Thread(target=self.listen, args=())
self.thread.setDaemon(True)
self.lock = threading.Lock()
self.thread.start()
self.q = PriorityQueue()
self.q_lock = threading.Lock()
self.replied_list = []
self.reply_lock = threading.Lock()
def send_to(self, message, node_id):
self.lock.acquire()
self.timestamp += 1
message = "<{},{}>:".format(self.timestamp, self.node_id) + message
logger.debug("node {} sends '{}' to node {} at {}".format(
self.node_id, message, node_id, self.timestamp))
self.lock.release()
self.nodeudp.send(message, ("127.0.0.1", startPort + node_id))
def broadcast(self, message):
global node_num
self.lock.acquire()
self.timestamp += 1
request_str = "<{},{}>".format(self.timestamp, self.node_id)
message = "<{},{}>:".format(self.timestamp, self.node_id) + message
logger.debug("node {} broadcasts '{}' at {}".format(
self.node_id, message, self.timestamp))
self.lock.release()
for node_id in range(node_num):
if node_id == self.node_id:
continue
self.nodeudp.send(message, ("127.0.0.1", startPort + node_id))
return request_str
def request(self):
request_str = self.broadcast("request")
request_tuple = str2tuple(request_str)
self.q_lock.acquire()
self.q.push(request_tuple)
self.q_lock.release()
def reply(self, request_str, to):
self.send_to("reply:" + request_str, to)
def release(self):
self.broadcast("release")
self.reply_lock.acquire()
self.replied_list = []
self.reply_lock.release()
self.q_lock.acquire()
self.q.pop()
self.q_lock.release()
def run(self):
global enter_times
global current
global node_num
for _ in range(enter_times):
logger.info("node {} requests to enter CS".format(self.node_id))
self.request()
while True:
time.sleep(0.1)
self.q_lock.acquire()
top_request_tuple = self.q.get()
# self.q.put(top_request_tuple)
self.q_lock.release()
self.reply_lock.acquire()
reply_num = len(self.replied_list)
self.reply_lock.release()
if self.node_id == top_request_tuple[1] and reply_num == (
node_num - 1):
break
logger.info("node {} enters CS at {}".format(
self.node_id, self.timestamp))
time.sleep(random.randint(1, 5))
self.release()
logger.info("node {} leaves CS at {}".format(
self.node_id, self.timestamp))
time.sleep(1)
time.sleep(3)
def listen(self):
while True:
msg = self.nodeudp.recv()[0]
ts = int(msg.split(',')[0][1:])
node_id = int(msg.split(',')[1].split('>')[0])
if ts > self.timestamp:
self.lock.acquire()
self.timestamp = ts + 1
self.lock.release()
else:
self.lock.acquire()
self.timestamp += 1
self.lock.release()
logger.debug("node {} recieve '{}' from node {} at {}".format(
self.node_id, msg, node_id, self.timestamp))
msg_type = msg.split(":")[1]
if msg_type == "request":
request_str = msg.split(":")[0]
self.reply(request_str, node_id)
request_tuple = str2tuple(request_str)
self.q_lock.acquire()
self.q.push(request_tuple)
self.q_lock.release()
elif msg_type == "reply":
self.reply_lock.acquire()
self.replied_list.append(node_id)
self.reply_lock.release()
elif msg_type == "release":
self.q_lock.acquire()
self.q.pop()
self.q_lock.release()
def sweep_line_algorithm(self):
self.current = Point()
pointsPQ = PriorityQueue()
tree = TreeSet()
pointsPQ.pushAll([seg.p for seg in self.segments])
pointsPQ.pushAll([seg.q for seg in self.segments])
res = 0
#print [str(x) for x in pointsPQ]
while not pointsPQ.isEmpty():
self.current.__update__(pointsPQ.pop())
#print "Round", current
if self.current.status == 'left':
#print "Adding", self.current.segment
low, high = tree.add_high_low(self.current.segment)
low = tree.lower(self.current.segment)
high = tree.higher(self.current.segment)
#print "Actual:", self.current.segment
#print "Low:", low, self.current.segment.intersect(low) if low else False
#print "High:", high, self.current.segment.intersect(high) if high else False
if low:
if self.current.segment.intersect(low):
a = self.current.segment.intersection_point(low)
#print "Adding a:", a, self.current.segment, low
pointsPQ.push(a)
if high:
if self.current.segment.intersect(high):
a = self.current.segment.intersection_point(high)
#print "Adding 2:", a, self.current.segment, high
pointsPQ.push(a)
elif self.current.status == "right":
low = tree.lower(self.current.segment)
high = tree.higher(self.current.segment)
if low and high:
if low.intersect(high):
a = low.intersection_point(high)
#print "Adding 3:", a, low, high
pointsPQ.push(a)
tree.remove(self.current.segment)
#print "Removing", self.current.segment
elif self.current.status == "int":
# exchange the position in tree of the two segments intersecting in current
s1, s2 = self.current.segment
#print "Between, swapping:", str(s1), str(s2)
tree.swap(s1, s2)
#print "After swap:", s1, s2, s1 is tree.lower(s2), s2 is tree.lower(s1)
#print "Modifying segments starts"
old_s1 = s1.p.node
old_s2 = s2.p.node
s1.set_p_node(self.current.node)
s2.set_p_node(self.current.node)
#print "Tree after modification:", [str(x) for x in tree]
# s1
if s1 is tree.lower(s2):
#print "... s1, s2, ..."
low = tree.lower(s1)
#print "s1:", s1, "low:", low, s1.intersect(low) if low else False
if low is not None:
if s1.intersect(low):
pointsPQ.push(s1.intersection_point(low))
high = tree.higher(s2)
#print "s2:", s2, "high:", high, s2.intersect(high) if high else False
if high is not None:
if s2.intersect(high):
pointsPQ.push(s2.intersection_point(high))
elif s2 is tree.lower(s1):
#print "... s2, s1, ..."
high = tree.higher(s1)
#print "s1:", s1, "high:", high, s1.intersect(high) if high else False
if high is not None:
if s1.intersect(high):
pointsPQ.push(s1.intersection_point(high))
low = tree.lower(s2)
#print "s2:", s2, "low:", low, s2.intersect(low) if low else False
if low is not None:
if s2.intersect(low):
pointsPQ.push(s2.intersection_point(low))
else:
print "Error" #raise SweepPlaneException("Intersection point error!")
res += 1
s1.set_p_node(old_s1)
s2.set_p_node(old_s2)
else:
print "Error 2" #raise SweepPlaneException("Node without status!")
#print "Tree", [str(x) for x in tree]
#print ""
self.nodes = self.nodes[:self.original_n_nodes]
return res
