Mapd now understands roundabouts

selfdrive/car/toyota/carstate.py

@@ -34,7 +34,7 @@ def gps_distance(gpsLat, gpsLon, gpsAlt, gpsAcc):
   speedlimit = float(B[minindex,7])
 
   if abs(gpsAlt -B[minindex,3]) < altacc:
-    if gpsAcc<1.00001:
+    if gpsAcc<0.1:
       #dist = 6371010*acos(sin(radians(gpsLat))*sin(radians(lat))+cos(radians(gpsLat))*cos(radians(lat))*cos(radians(gpsLon-lon)))
       dist = (np.sum((B[minindex,[0,1,2]] - A)**2))**0.5
   #else:

selfdrive/controls/lib/planner.py

@@ -224,13 +224,13 @@ def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data):
       # Change accel limits based on time remaining to turn
       if decel_for_turn:
         time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max((v_ego + v_curvature)/2, 1.))
-        required_decel = min(0, (v_curvature - v_ego) / time_to_turn)
+        required_decel = min(0, (v_curvature - v_ego) / time_to_turn*0.85)
         accel_limits[0] = max(accel_limits[0], required_decel)
 
       if v_speedlimit_ahead < v_speedlimit:
         if live_map_data.liveMapData.speedLimitAheadDistance != 0:
           required_decel = min(0, (v_speedlimit_ahead*v_speedlimit_ahead - v_ego*v_ego)/(live_map_data.liveMapData.speedLimitAheadDistance*2))
-        required_decel = max(required_decel, -3.0)
+        required_decel = max(required_decel*0.85, -3.0)
         #print "required_decel"
         #print required_decel
         #print "accel_limits 0"

selfdrive/mapd/mapd_helpers.py

@@ -236,7 +236,26 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
       if max_dist > 2 * lookahead:
         #print "max_dist break"
         break
-
+      try:
+        if way.way.tags['junction']=='roundabout':
+          latmin = 181
+          lonmin = 181
+          latmax = -181
+          lonmax = -181
+          for n in way.way.nodes:
+            lonmax = max(n.lon,lonmax)
+            lonmin = min(n.lon,lonmin)
+            latmax = max(n.lat,latmax)
+            latmin = min(n.lat,latmin)
+          a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(lonmax-lonmin)
+          speed_ahead = np.sqrt(1.6075*a)
+          min_dist = 999.9
+          for w in way_pts:
+            min_dist = min(min_dist, float(np.linalg.norm(w)))
+          speed_ahead_dist = min_dist
+          break
+      except KeyError:
+        pass
       if 'maxspeed' in way.way.tags:
         spd = parse_speed_tags(way.way.tags)
         #print "spd found"
@@ -340,12 +359,23 @@ def next_way(self, heading):
     try:
       # Simple heuristic to find next way
       ways = [w for w in ways if w.id != self.id]
+      if len(ways) == 1:
+        way = Way(ways[0], self.query_results)
+        #print "only one way found"
+        return way
       ways = [w for w in ways if (w.nodes[0] == node or w.nodes[-1] == node)]
       if len(ways) == 1:
         way = Way(ways[0], self.query_results)
         #print "only one way found"
         return way
-
+      if len(ways) == 2:
+        try:
+          if ways[0].tags['junction']=='roundabout':
+            #print ("roundabout found")
+            way = Way(ways[0], self.query_results)
+            return way
+        except KeyError:
+          pass
       # Filter on highway tag
       acceptable_tags = list()
       cur_tag = self.way.tags['highway']