From ac5b350fb12f4d16ffc32836d439fc54ff7be084 Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Fri, 24 Jul 2020 16:05:17 -0700
Subject: [PATCH] anytimeD

---
 .../Search_3D/Anytime_Dstar3D.py              |  30 +++++++++++-------
 .../__pycache__/queue.cpython-37.pyc          | Bin 4619 -> 4833 bytes
 Search-based Planning/Search_3D/queue.py      |   7 ++++
 3 files changed, 25 insertions(+), 12 deletions(-)

diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py
index 80219a8..2af528c 100644
--- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py	
+++ b/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()
\ No newline at end of file
diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc
index aaa3e20598b0d52bfa0d12a7d3c4420ef70a4537..aaa219f937078d808dabe03d4e6c75be2cf61dd8 100644
GIT binary patch
delta 637
zcmYk3J#W-N5QcZW{@8PD-(3X9<syiLa=AD}1OWwAB!VI#2q7UzbU9tlRz8UCl0Ab$
zWQigrP|Y83S|o({2~-rcRIZ?;;0I8_jI9LL+Gof6G4DIOZ>E2=gC*a01+{nElU=L!
zDflpkzwO6pcwc8{X?k0vqAL<O;nCP-%EZj9Q;zHihz|bpnst>M2Zy;F#Yt`rhEY61
z*MCweQiBZ&`~Cu~A^P{+RO~sKO5qgno4>qN(X6Ad5^{48A0_%4|2G*Ej5eb~$Yn2z
zLQC5`(PB(8=5Rf@(s)YYC?E*Kg|jcflVApJ<NKiVnq*~WvMh*5Y>3cU{rQI)B`gdy
zIM+Obwe%Drl>cch@T_wV#Sxp{q&_O^bio9V#E%<~b_Y9g?_h2HdVeSGKkw=o?^Ijf
z3hg~wBqu^I;%2pdQS4cnol5qU$_;U7JQIn7Z>ulws<Q6l8ZNU|VXbg_?k`sl<5$aC
z4|QmkPRk47?JXQsE?tP(TzM-B#>tuj54UO&4Dm~C3pCEvr!cF`KnK4&6Z5(1?e7<_
z_ks@^1Odn6|Ef7#UJb`^rG5);;zzrAW16<z=L0o@GEGp9a*HmN)VvxxTGCLyYKWif
G5B>s1;(*Wq

delta 457
zcmYL_Jxc>Y5QcZ}`;z-ej$pnB2FYD=q6Fg~2#J+Nid<nM1VVzS5X|O~2(hrXQKzsH
zwAUh~|G>i9B9%6Rh=o5uboL@T%RbD$3^VWSi+ftK8m1W|81GNnPTQVZcX3)jzgmNj
zctQ$@Z-Z_;7K>Q0Lnfp{b~$M%T#Gk6lX?^>!<Uhfm6>ob1kY@%8gEDyI5Deq1Eyy4
z!fG~D=Ap`#5Ho4eABW86?+izl!$tTqQ+S@^$a7Q>Gr2zq*%5rosZ@aPk%1sWjQ$?D
zw>(;bXRF-q5i~8d-o7z>aXxYZi`g;)h5zm=Tvh+TJxAuYF&WJ_h*60eFtn>i9mR2N
zLiPx&!L9A;VnBw%771l|wGY!uWPZL&fj8pQm<Pv+oWd7iIT+|^r;F*Yca1veUW123
xkM6^2vP%bWmRuBDehl)?Is{I^wXj^1B&sQz9$k@@Q1VqK@}AMpmEqFa`~fQ*Rqy}+

diff --git a/Search-based Planning/Search_3D/queue.py b/Search-based Planning/Search_3D/queue.py
index b4e4c0b..f49ed26 100644
--- a/Search-based Planning/Search_3D/queue.py	
+++ b/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