anytimeD

Search-based Planning/Search_3D/Anytime_Dstar3D.py

@@ -52,6 +52,7 @@ class Anytime_Dstar(object):
         # epsilon in the key caculation
         self.epsilon = 1
         self.increment = 0.1
+        self.decrement = 0.2
 
     def getcost(self, xi, xj):
         # use a LUT for getting the costd
@@ -131,9 +132,12 @@ class Anytime_Dstar(object):
                 self.INCONS.add(s)
 
     def ComputeorImprovePath(self):
-        while self.OPEN.top_key() < self.key(self.x0) or self.rhs[self.x0] != self.g[self.x0]:
+        while self.OPEN.top_key() < self.key(self.x0,self.epsilon) or self.rhs[self.x0] != self.g[self.x0]:
             s = self.OPEN.get()
-            visualization(self)
+
+            if getDist(s, tuple(self.env.start)) < self.env.resolution:
+                break
+
             if self.g[s] > self.rhs[s]:
                 self.g[s] = self.rhs[s]
                 self.CLOSED.add(s)
@@ -148,13 +152,15 @@ class Anytime_Dstar(object):
             self.ind += 1
 
     def Main(self):
-        epsilon = self.epsilon
-        increment = self.increment
         ischanged = False
         islargelychanged = False
+        t = 0
         self.ComputeorImprovePath()
         #TODO publish current epsilon sub-optimal solution
         while True:
+            print(t)
+            if t == 5:
+                break
             # change environment
             new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation')
             ischanged = True
@@ -170,23 +176,23 @@ class Anytime_Dstar(object):
                 ischanged = False
 
             if islargelychanged:  
-                epsilon += increment # or replan from scratch
-            elif epsilon > 1:
-                epsilon -= increment
+                self.epsilon += self.increment # or replan from scratch
+            elif self.epsilon > 1:
+                self.epsilon -= self.decrement
             
             # move states from the INCONS to OPEN
             # update priorities in OPEN
-            Allnodes = self.INCONS.union(set(self.OPEN.enumerate()))
+            Allnodes = self.INCONS.union(self.OPEN.allnodes())
             for node in Allnodes:
-                self.OPEN.put(node, self.key(node, epsilon)) 
+                self.OPEN.put(node, self.key(node, self.epsilon)) 
             self.INCONS = set()
             self.CLOSED = set()
             self.ComputeorImprovePath()
-            # publish current epsilon sub optimal solution
+            #TODO publish current epsilon sub optimal solution
             # if epsilon == 1:
                 # wait for change to occur
-        pass
+            t += 1
 
 if __name__ == '__main__':
-    AD = Anytime_Dstar(resolution = 0.5)
+    AD = Anytime_Dstar(resolution = 1)
     AD.Main()

Search-based Planning/Search_3D/queue.py

@@ -76,6 +76,7 @@ class MinheapPQ:
     """
     def __init__(self):
         self.pq = [] # lis of the entries arranged in a heap
+        self.nodes = set()
         self.entry_finder = {} # mapping of the item entries
         self.counter = itertools.count() # unique sequence count
         self.REMOVED = '<removed-item>'
@@ -88,12 +89,14 @@ class MinheapPQ:
         entry = [priority, count, item]
         self.entry_finder[item] = entry
         heapq.heappush(self.pq, entry)
+        self.nodes.add(item)
 
     def check_remove(self, item):
         if item not in self.entry_finder:
             return
         entry = self.entry_finder.pop(item)
         entry[-1] = self.REMOVED
+        self.nodes.remove(item)
 
     def get(self):
         """Remove and return the lowest priority task. Raise KeyError if empty."""
@@ -101,6 +104,7 @@ class MinheapPQ:
             priority, count, item = heapq.heappop(self.pq)
             if item is not self.REMOVED:
                 del self.entry_finder[item]
+                self.nodes.remove(item)
                 return item
         raise KeyError('pop from an empty priority queue')
 
@@ -110,6 +114,9 @@ class MinheapPQ:
     def enumerate(self):
         return self.pq
 
+    def allnodes(self):
+        return self.nodes
+
 # class QueuePrior:
 #     """
 #     Class: QueuePrior