Updating policy to use new local resolver class

guided_mrmp/planners/multirobot/db_guided_mrmp.py

@@ -11,7 +11,7 @@ from shapely.geometry import Polygon, Point
 from guided_mrmp.utils import Env
 from guided_mrmp.utils.helpers import *
 
-from guided_mrmp.conflict_resolvers import TrajOptDBResolver
+from guided_mrmp.conflict_resolvers import TrajOptResolver,TrajOptDBResolver
 
 from guided_mrmp.utils import Roomba
 
@@ -32,7 +32,6 @@ class GuidedMRMP:
         self.current_guides = []*len(robots)
         self.current_trajs = []*len(robots)
 
-
     def find_all_conflicts(self, dt):
         """
         Loop over all the robots, checking for both node conflicts and edge
@@ -86,10 +85,14 @@ class GuidedMRMP:
         conflicts = self.find_all_conflicts(dt)
 
         # resolve the conflicts using the database
-        resolver = TrajOptDBResolver()
-        resolver.resolve_conflict(conflicts, screen)
-
+        # resolver = TrajOptDBResolver(10, 60, conflicts, self.robots)
+        resolver = TrajOptResolver(conflicts, self.robots, dt, 10,)
+        updated_controls = resolver.get_local_controls()
 
+        for conflict,new_controls in zip(conflicts,updated_controls):
+            for r,control in zip(conflict,new_controls):
+                r.next_control = control
+            
         # update the state of each robot
 
         # for idx, r in enumerate(self.robots):
@@ -135,9 +138,6 @@ class GuidedMRMP:
 
 if __name__ == "__main__":
 
-
-    # set_python_seed(42)
-
     # create the environment
     env = Env()