Merge pull request #45 from andycui97/master

Fix geo_lite planner
BerkeleyAutomation · Apr 5, 2018 · 6eb0f3b · 6eb0f3b
2 parents 1c56660 + 70c1c80
commit 6eb0f3b
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 4 deletions.
diff --git a/gym_urbandriving/planning/geometric_planner_lite.py b/gym_urbandriving/planning/geometric_planner_lite.py
@@ -4,7 +4,15 @@
 
 class GeometricPlanner:
     def __init__(self,state, inter_point_d=1.0, planning_time=1.0, optional_targets = None, num_cars = 0):
-        pass
+        if optional_targets is None:
+            self.optional_targets = [[450,375,-np.pi/2],
+                           [550,375,np.pi/2],
+                           [625,450,-np.pi],
+                           [625,550,0.0],
+                           [450,625,-np.pi/2],
+                           [550,625,np.pi/2],
+                           [375,450,-np.pi],
+                           [375,550,0.0]]
 
     def plan(self,x0,y0,v0,a0,x1,y1,v1,a1):
         def interpolate(p0,p1,p2,p3,t):
@@ -34,8 +42,15 @@ def plan_for_agents(self, state,type_of_agent='background_cars',agent_num=0):
 
         obj =  state.dynamic_objects[type_of_agent][str(agent_num)]
 
-        traj = Trajectory(mode = 'xyv', fsm=0)
-        for p in self.plan(obj.x, obj.y, obj.vel, obj.angle, obj.destination[0], obj.destination[1], 1, obj.destination[3]):
+        target1 = sorted(self.optional_targets, key=lambda p: (p[0]-obj.x)**2 + (p[1]-obj.y)**2)[0]
+        target2 = sorted(self.optional_targets, key=lambda p: (p[0]-obj.destination[0])**2 + (p[1]-obj.destination[1])**2)[0]
+
+        traj = Trajectory(mode = 'xyv')
+        for p in self.plan(obj.x, obj.y, obj.vel, obj.angle, target1[0], target1[1], 1, target1[2]):
+            traj.add_point(p)
+        for p in self.plan(target1[0], target1[1], 1, target1[2], target2[0], target2[1], 1, target2[2]):
+            traj.add_point(p)
+        for p in self.plan(target2[0], target2[1], 1, target2[2], obj.destination[0], obj.destination[1], 1, obj.destination[3]):
             traj.add_point(p)
 
         obj.trajectory = traj

diff --git a/gym_urbandriving/state/configs/four_way_intersection.json b/gym_urbandriving/state/configs/four_way_intersection.json
@@ -38,7 +38,7 @@
         { "x":550, "y":100, "vel":2, "angle_deg":90 },
         { "x":450, "y":900, "vel":2, "angle_deg":-90 },
         { "x":900, "y":550, "vel":2, "angle_deg":0 },
-        { "x":100, "y":450, "vel":2, "angle_deg":90 }
+        { "x":100, "y":450, "vel":2, "angle_deg":180 }
     ]
 }