'BITstar'

Sampling_based_Planning/rrt_3D/queue.py

@@ -26,6 +26,11 @@ class MinheapPQ:
         heapq.heappush(self.pq, entry)
         self.nodes.add(item)
 
+    def put_set(self, dictin):
+        '''add a new dict into the priority queue'''
+        for item, priority in enumerate(dictin):
+            self.put(item, priority)
+
     def check_remove(self, item):
         if item not in self.entry_finder:
             return
@@ -33,6 +38,34 @@ class MinheapPQ:
         entry[-1] = self.REMOVED
         self.nodes.remove(item)
 
+    def check_remove_set(self, set_input):
+        if len(set_input) == 0:
+            return
+        for item in set_input:
+            if item not in self.entry_finder:
+                continue
+            entry = self.entry_finder.pop(item)
+            entry[-1] = self.REMOVED
+            self.nodes.remove(item)
+
+    def priority_filtering(self, threshold, mode):
+        # mode: bigger: check and remove those key vals bigger than threshold
+        if mode == 'lowpass':
+            for entry in self.enumerate():
+                item = entry[2]
+                if entry[0] >= threshold: # priority
+                    _ = self.entry_finder.pop(item)
+                    entry[-1] = self.REMOVED
+                    self.nodes.remove(item)
+        # mode: smaller: check and remove those key vals smaller than threshold
+        elif mode == 'highpass':
+            for entry in self.enumerate():
+                item = entry[2]
+                if entry[0] <= threshold: # priority
+                    _ = 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."""
         while self.pq: