mapbox navi

cereal/log.capnp

@@ -2615,8 +2615,8 @@ struct Event {
     # DO change the name of the field
     # DON'T change anything after the "@"
     customReservedRawData0 @124 :Data;
-    customReservedRawData1 @125 :Data;
+    navRouteNavd @125 :NavRoute;
-    customReservedRawData2 @126 :Data;
+    navInstructionCarrot @126 :NavInstruction;
 
     # *********** Custom: reserved for forks ***********
     carrotMan @107 :Custom.CarrotMan;

cereal/services.py

@@ -71,6 +71,7 @@ _services: dict[str, tuple] = {
   "uploaderState": (True, 0., 1),
   "navInstruction": (True, 1., 10),
   "navRoute": (True, 0.),
+  "navRouteNavd": (True, 0.),
   "navThumbnail": (True, 0.),
   "navModel": (True, 2., 4.),
   "mapRenderState": (True, 2., 1.),
@@ -79,6 +80,7 @@ _services: dict[str, tuple] = {
   "microphone": (True, 10., 10),
 
   "carrotMan": (True, 0.),
+  "navInstructionCarrot": (True, 1., 10),
 
   # debug
   "uiDebug": (True, 0., 1),

selfdrive/car/cruise.py

@@ -542,11 +542,8 @@ class VCruiseCarrot:
     if road_limit_kph < 1.0:
       return v_cruise_kph
 
-    desired_speed = min(self.desiredSpeed, road_limit_kph)
+    if self.v_lead_kph + 5 > v_cruise_kph and v_cruise_kph < road_limit_kph and self.d_rel < 60:
-    if self.v_lead_kph + 5 > v_cruise_kph and v_cruise_kph < desired_speed and self.d_rel < 60:
+      v_cruise_kph = min(v_cruise_kph + 5, road_limit_kph)
-      v_cruise_kph += 5
-    #elif self.model_v_kph + 5 > v_cruise_kph and v_cruise_kph < desired_speed:
-    #  v_cruise_kph += 5
     elif road_limit_kph < self.road_limit_kph:
       new_road_limit_kph = road_limit_kph * self.autoRoadSpeedAdjust + self.road_limit_kph * (1 - self.autoRoadSpeedAdjust)
       v_cruise_kph = min(v_cruise_kph, new_road_limit_kph)

selfdrive/carrot/carrot_man.py

@@ -179,10 +179,11 @@ def calculate_curvature(p1, p2, p3):
 
 class CarrotMan:
   def __init__(self):
+    print("************************************************CarrotMan init************************************************")
     self.params = Params()
     self.params_memory = Params("/dev/shm/params")
-    self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl'])
+    self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd'])
-    self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstruction"])
+    self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"])
 
     self.carrot_serv = CarrotServ()
 
@@ -217,6 +218,7 @@ class CarrotMan:
     self.navi_points = []
     self.navi_points_start_index = 0
     self.navi_points_active = False
+    self.navd_active = False
 
     self.active_carrot_last = False
 
@@ -257,6 +259,8 @@ class CarrotMan:
     while self.is_running:
       try:
         self.sm.update(0)
+        if self.sm.updated['navRouteNavd']:
+          self.send_routes(self.sm['navRouteNavd'].coordinates, True)
         remote_addr = self.remote_addr
         remote_ip = remote_addr[0] if remote_addr is not None else ""
         vturn_speed = self.carrot_curve_speed(self.sm)
@@ -290,6 +294,7 @@ class CarrotMan:
 
             if remote_addr is None:
               print(f"Broadcasting: {self.broadcast_ip}:{msg}")
+              if not self.navd_active:
                 self.navi_points = []
                 self.navi_points_active = False
 
@@ -309,14 +314,15 @@ class CarrotMan:
 
   def carrot_navi_route(self):
 
-    if not self.navi_points_active or not SHAPELY_AVAILABLE or self.carrot_serv.active_carrot <= 1:
+    if not self.navi_points_active or not SHAPELY_AVAILABLE or (self.carrot_serv.active_carrot <= 1 and not self.navd_active):
       #print(f"navi_points_active: {self.navi_points_active}, active_carrot: {self.carrot_serv.active_carrot}")
       #haversine_cache.clear()
       #curvature_cache.clear()
       self.navi_points = []
       self.navi_points_active = False
       if self.active_carrot_last > 1:
-        self.params.remove("NavDestination")
+        #self.params.remove("NavDestination")
+        pass
       self.active_carrot_last = self.carrot_serv.active_carrot
       return [],[],300
 
@@ -397,10 +403,11 @@ class CarrotMan:
             #print(f"out_speeds= {[round(s, 1) for s in out_speeds]}")
     else:
         resampled_points = []
+        resampled_distances = []
         curvatures = []
         speeds = []
         distances = []
-        self.params.remove("NavDestination")
+        #self.params.remove("NavDestination")
 
     return resampled_points, resampled_distances, out_speed #speeds, distances
 
@@ -677,6 +684,25 @@ class CarrotMan:
     return struct.unpack('!f', float_data)[0]
 
 
+  def send_routes(self, coords, from_navd=False):
+    if from_navd:
+      if len(coords) > 0:
+        self.navi_points = [(c.longitude, c.latitude) for c in coords]
+        self.navi_points_start_index = 0
+        self.navi_points_active = True
+        print("Received points from navd:", len(self.navi_points))
+        self.navd_active = True
+
+        coords = [{"latitude": c.latitude, "longitude": c.longitude} for c in coords]
+        #print("navdNaviPoints=", self.navi_points)
+      else:
+        print("Received points from navd: 0")
+        self.navd_active = False
+
+    msg = messaging.new_message('navRoute', valid=True)
+    msg.navRoute.coordinates = coords
+    self.pm.send('navRoute', msg)
+
   def carrot_route(self):
     host = '0.0.0.0'  # 혹은 다른 호스트 주소
     port = 7709  # 포트 번호
@@ -718,11 +744,23 @@ class CarrotMan:
             print("Received points:", len(self.navi_points))
             #print("Received points:", self.navi_points)
 
-            msg = messaging.new_message('navRoute', valid=True)
+            self.send_routes(coords)
-            msg.navRoute.coordinates = coords
+            """
-            self.pm.send('navRoute', msg)
+            try:
-            #self.carrot_route_active = True
+              module_name = "route_engine"
-            #self.params.put_bool_nonblocking("CarrotRouteActive", True)
+              class_name = "RouteEngine"
+              moduel = importlib.import_module(module_name)
+              cls = getattr(moduel, class_name)
+              route_engine_instance = cls(name="Loaded at Runtime")
+
+              route_engine_instance.send_route_coords(coords, True)
+            except Exception as e:
+              print(f"route_engine error: {e}")
+
+            #msg = messaging.new_message('navRoute', valid=True)
+            #msg.navRoute.coordinates = coords
+            #self.pm.send('navRoute', msg)
+            """
 
             if len(coords):
               dest = coords[-1]
@@ -1588,10 +1626,10 @@ class CarrotServ:
       249: ("", "", 6)   #TG
     }
 
-    msg = messaging.new_message('navInstruction')
+    msg = messaging.new_message('navInstructionCarrot')
     msg.valid = True
     
-    instruction = msg.navInstruction
+    instruction = msg.navInstructionCarrot
     instruction.distanceRemaining = self.nGoPosDist
     instruction.timeRemaining = self.nGoPosTime
     instruction.speedLimit = self.nRoadLimitSpeed / 3.6 if self.nRoadLimitSpeed > 0 else 0
@@ -1628,7 +1666,7 @@ class CarrotServ:
 
     instruction.allManeuvers = maneuvers
 
-    pm.send('navInstruction', msg)
+    pm.send('navInstructionCarrot', msg)
 
   def _update_system_time(self, epoch_time_remote, timezone_remote):
     epoch_time = int(time.time())
@@ -1781,6 +1819,8 @@ def main():
   print("CarrotManager Started")
   #print("Carrot GitBranch = {}, {}".format(Params().get("GitBranch"), Params().get("GitCommitDate")))
   carrot_man = CarrotMan()
+
+  print(f"CarrotMan {carrot_man}")
   while True:
     try:
       carrot_man.carrot_man_thread()

selfdrive/frogpilot/fleetmanager/helpers.py

@@ -329,6 +329,7 @@ def set_destination(postvars, valid_addr):
   return postvars, valid_addr
 
 def nav_confirmed(postvars):
+  print(f"nav_confirmed {postvars}")
   if postvars is not None:
     lat = float(postvars.get("lat"))
     lng = float(postvars.get("lon"))
@@ -337,6 +338,7 @@ def nav_confirmed(postvars):
     if params.get_int("SearchInput") == 1:
       lng, lat = gcj02towgs84(lng, lat)
     params.put("NavDestination", f'{{"latitude": {lat:.6f}, "longitude": {lng:.6f}, "place_name": "{name}"}}')
+    print(f"nav_confirmed {lat}, {lng}, {name}")
     if name == "":
       name =  str(lat) + "," + str(lng)
     new_dest = {"latitude": float(lat), "longitude": float(lng), "place_name": name}

selfdrive/modeld/modeld.py

@@ -155,7 +155,7 @@ def main(demo=False):
 
   # messaging
   pm = PubMaster(["modelV2", "cameraOdometry"])
-  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "carrotMan", "radarState", "navModel", "navInstruction"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "carrotMan", "radarState", "navModel", "navInstruction", "navInstructionCarrot"])
 
   publish_state = PublishState()
   params = Params()
@@ -256,9 +256,10 @@ def main(demo=False):
     if nav_enabled and sm.updated["navModel"]:
       nav_features = np.array(sm["navModel"].features)
 
-    if nav_enabled and sm.updated["navInstruction"]:
+    if nav_enabled and (sm.updated["navInstruction"] or sm.updated["navInstructionCarrot"]):
       nav_instructions[:] = 0
-      for maneuver in sm["navInstruction"].allManeuvers:
+      navInstructions = sm["navInstructionCarrot"] if sm.updated["navInstructionCarrot"] else sm["navInstruction"]
+      for maneuver in navInstructions.allManeuvers:
         distance_idx = 25 + int(maneuver.distance / 20)
         direction_idx = 0
         if maneuver.modifier in ("left", "slight left", "sharp left"):

selfdrive/modeld/navmodeld.py

@@ -98,7 +98,7 @@ def main():
   assert vipc_client.is_connected()
   cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
 
-  sm = SubMaster(["navInstruction"])
+  sm = SubMaster(["navInstruction", "navInstructionCarrot"])
   pm = PubMaster(["navModel"])
 
   while True:
@@ -111,7 +111,7 @@ def main():
     model_output, dsp_execution_time = model.run(buf.data[:buf.uv_offset])
     t2 = time.perf_counter()
 
-    valid = vipc_client.valid and sm.valid["navInstruction"]
+    valid = vipc_client.valid and (sm.valid["navInstruction"] or sm.valid["navInstructionCarrot"])
     pm.send("navModel", get_navmodel_packet(model_output, valid, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time))
 
 

selfdrive/navd/helpers.py

@@ -106,19 +106,19 @@ def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> floa
   return total_distance_closest
 
 
-def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]:
+def coordinate_from_param(param: str, params: Optional[Params] = None) -> Tuple[Optional[Coordinate], Optional[str]]:
   if params is None:
     params = Params()
 
   json_str = params.get(param)
   if json_str is None:
-    return None
+    return None, None
 
   pos = json.loads(json_str)
   if 'latitude' not in pos or 'longitude' not in pos:
-    return None
+    return None, None
-
+  place_name = pos.get('place_name', None)
-  return Coordinate(pos['latitude'], pos['longitude'])
+  return Coordinate(pos['latitude'], pos['longitude']), place_name
 
 
 def string_to_direction(direction: str) -> str:
@@ -156,6 +156,7 @@ def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -
   instruction['showFull'] = distance_to_maneuver < current_banner['distanceAlongGeometry']
 
   # Primary
+  #print(f"current_banner: {current_banner}")
   p = current_banner['primary']
   if field_valid(p, 'text'):
     instruction['maneuverPrimaryText'] = p['text']

selfdrive/navd/navd.py

@@ -0,0 +1,394 @@
+#!/usr/bin/env python3
+import json
+import math
+import os
+import threading
+import importlib
+
+import requests
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import log
+from openpilot.common.api import Api
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
+                                    distance_along_geometry, maxspeed_to_ms,
+                                    minimum_distance,
+                                    parse_banner_instructions)
+from openpilot.common.swaglog import cloudlog
+from selfdrive.carrot.carrot_man import CarrotMan
+
+
+REROUTE_DISTANCE = 25
+MANEUVER_TRANSITION_THRESHOLD = 10
+REROUTE_COUNTER_MIN = 3
+
+
+class RouteEngine:
+  def __init__(self, sm, pm):
+    self.name = "RouteEngine Instance"
+    self.sm = sm
+    self.pm = pm
+
+    self.params = Params()
+
+    # Get last gps position from params
+    self.last_position,_ = coordinate_from_param("LastGPSPosition", self.params)
+    self.last_bearing = None
+
+    self.gps_ok = False
+    self.localizer_valid = False
+
+    self.nav_destination = None
+    self.step_idx = None
+    self.route = None
+    self.route_geometry = None
+
+    self.recompute_backoff = 0
+    self.recompute_countdown = 0
+
+    self.ui_pid = None
+
+    self.reroute_counter = 0
+
+    self.carrot_route_active = False
+
+    if "MAPBOX_TOKEN" in os.environ:
+      self.mapbox_token = os.environ["MAPBOX_TOKEN"]
+      self.mapbox_host = "https://api.mapbox.com"
+    elif self.params.get_int("PrimeType") == 0:
+      self.mapbox_token = self.params.get("MapboxPublicKey", encoding='utf8')
+      self.mapbox_host = "https://api.mapbox.com"
+    else:
+      try:
+        self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24)
+      except FileNotFoundError:
+        cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.")
+        self.mapbox_token = ""
+      self.mapbox_host = "https://maps.comma.ai"
+
+  def update(self):
+    self.sm.update(0)
+
+    if self.sm.updated["managerState"]:
+      ui_pid = [p.pid for p in self.sm["managerState"].processes if p.name == "ui" and p.running]
+      if ui_pid:
+        if self.ui_pid and self.ui_pid != ui_pid[0]:
+          cloudlog.warning("UI restarting, sending route")
+          print("########## UI restarting, sending route")
+          threading.Timer(5.0, self.send_route).start()
+        self.ui_pid = ui_pid[0]
+
+    self.update_location()
+
+    if self.carrot_route_active:
+      return
+
+    try:
+      self.recompute_route()
+      self.send_instruction()
+    except Exception:
+      cloudlog.exception("navd.failed_to_compute")
+
+  def update_location(self):
+    location = self.sm['liveLocationKalman']
+    self.gps_ok = location.gpsOK
+
+    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
+
+    carrot_man = self.sm['carrotMan']
+    if carrot_man.activeCarrot > 1:
+      self.last_bearing = carrot_man.xPosAngle;
+      self.last_position = Coordinate(carrot_man.xPosLat, carrot_man.xPosLon)
+      self.gps_ok = True
+    elif self.localizer_valid:
+      self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
+      self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
+      self.carrot_route_active = False
+
+  def recompute_route(self):
+    if self.last_position is None:
+      return
+
+    new_destination, place_name = coordinate_from_param("NavDestination", self.params)
+    if new_destination is None:
+      self.clear_route()
+      self.reset_recompute_limits()
+      return
+    #print(f"new_destination: {new_destination} nav_destination: {self.nav_destination}")
+    should_recompute = self.should_recompute()
+    if new_destination != self.nav_destination and place_name != "External Navi":
+      cloudlog.warning(f"Got new destination from NavDestination param {new_destination}")
+      print(f"Got new destination from NavDestination param {new_destination} {self.nav_destination}")
+      should_recompute = True
+
+    # Don't recompute when GPS drifts in tunnels
+    if not self.gps_ok and self.step_idx is not None:
+      return
+
+    if self.recompute_countdown == 0 and should_recompute:
+      self.recompute_countdown = 2**self.recompute_backoff
+      self.recompute_backoff = min(6, self.recompute_backoff + 1)
+      self.calculate_route(new_destination)
+      self.reroute_counter = 0
+    else:
+      self.recompute_countdown = max(0, self.recompute_countdown - 1)
+
+  def calculate_route(self, destination):
+    cloudlog.warning(f"Calculating route {self.last_position} -> {destination}")
+    print(f"############## Calculating route {self.last_position} -> {destination}")
+    self.nav_destination = destination
+
+    lang = self.params.get('LanguageSetting', encoding='utf8')
+    if lang is not None:
+      lang = lang.replace('main_', '')
+
+    params = {
+      'access_token': self.mapbox_token,
+      'annotations': 'maxspeed',
+      'geometries': 'geojson',
+      'overview': 'full',
+      'steps': 'true',
+      'banner_instructions': 'true',
+      'alternatives': 'false',
+      'language': lang,
+    }
+
+    # TODO: move waypoints into NavDestination param?
+    waypoints = self.params.get('NavDestinationWaypoints', encoding='utf8')
+    waypoint_coords = []
+    if waypoints is not None and len(waypoints) > 0:
+      waypoint_coords = json.loads(waypoints)
+
+    coords = [
+      (self.last_position.longitude, self.last_position.latitude),
+      *waypoint_coords,
+      (destination.longitude, destination.latitude)
+    ]
+    params['waypoints'] = f'0;{len(coords)-1}'
+    if self.last_bearing is not None:
+      params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90" + (';'*(len(coords)-1))
+
+    coords_str = ';'.join([f'{lon},{lat}' for lon, lat in coords])
+    url = self.mapbox_host + '/directions/v5/mapbox/driving-traffic/' + coords_str
+    try:
+      resp = requests.get(url, params=params, timeout=10)
+      if resp.status_code != 200:
+        cloudlog.event("API request failed", status_code=resp.status_code, text=resp.text, error=True)
+      resp.raise_for_status()
+
+      r = resp.json()
+      if len(r['routes']):
+        self.route = r['routes'][0]['legs'][0]['steps']
+        self.route_geometry = []
+
+        maxspeed_idx = 0
+        maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed']
+
+        # Convert coordinates
+        for step in self.route:
+          coords = []
+
+          for c in step['geometry']['coordinates']:
+            coord = Coordinate.from_mapbox_tuple(c)
+
+            # Last step does not have maxspeed
+            if (maxspeed_idx < len(maxspeeds)):
+              maxspeed = maxspeeds[maxspeed_idx]
+              if ('unknown' not in maxspeed) and ('none' not in maxspeed):
+                coord.annotations['maxspeed'] = maxspeed_to_ms(maxspeed)
+
+            coords.append(coord)
+            maxspeed_idx += 1
+
+          self.route_geometry.append(coords)
+          maxspeed_idx -= 1  # Every segment ends with the same coordinate as the start of the next
+
+        self.step_idx = 0
+      else:
+        cloudlog.warning("Got empty route response")
+        print("Got empty route response")
+        self.clear_route()
+
+      # clear waypoints to avoid a re-route including past waypoints
+      # TODO: only clear once we're past a waypoint
+      self.params.remove('NavDestinationWaypoints')
+
+    except requests.exceptions.RequestException:
+      cloudlog.exception("failed to get route")
+      print("failed to get route")
+      self.clear_route()
+
+    self.send_route()
+
+  def send_instruction(self):
+    msg = messaging.new_message('navInstruction', valid=True)
+    #print("navd_navInstruction")
+    if self.step_idx is None:
+      msg.valid = False
+      self.pm.send('navInstruction', msg)
+      return
+
+    step = self.route[self.step_idx]
+    geometry = self.route_geometry[self.step_idx]
+    along_geometry = distance_along_geometry(geometry, self.last_position)
+    distance_to_maneuver_along_geometry = step['distance'] - along_geometry
+
+    # Banner instructions are for the following maneuver step, don't use empty last step
+    banner_step = step
+    if not len(banner_step['bannerInstructions']) and self.step_idx == len(self.route) - 1:
+      banner_step = self.route[max(self.step_idx - 1, 0)]
+
+    # Current instruction
+    msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
+    instruction = parse_banner_instructions(banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
+    if instruction is not None:
+      for k,v in instruction.items():
+        setattr(msg.navInstruction, k, v)
+
+    # All instructions
+    maneuvers = []
+    for i, step_i in enumerate(self.route):
+      if i < self.step_idx:
+        distance_to_maneuver = -sum(self.route[j]['distance'] for j in range(i+1, self.step_idx)) - along_geometry
+      elif i == self.step_idx:
+        distance_to_maneuver = distance_to_maneuver_along_geometry
+      else:
+        distance_to_maneuver = distance_to_maneuver_along_geometry + sum(self.route[j]['distance'] for j in range(self.step_idx+1, i+1))
+
+      instruction = parse_banner_instructions(step_i['bannerInstructions'], distance_to_maneuver)
+      if instruction is None:
+        continue
+      maneuver = {'distance': distance_to_maneuver}
+      if 'maneuverType' in instruction:
+        maneuver['type'] = instruction['maneuverType']
+      if 'maneuverModifier' in instruction:
+        maneuver['modifier'] = instruction['maneuverModifier']
+      maneuvers.append(maneuver)
+
+    msg.navInstruction.allManeuvers = maneuvers
+
+    # Compute total remaining time and distance
+    remaining = 1.0 - along_geometry / max(step['distance'], 1)
+    total_distance = step['distance'] * remaining
+    total_time = step['duration'] * remaining
+
+    if step['duration_typical'] is None:
+      total_time_typical = total_time
+    else:
+      total_time_typical = step['duration_typical'] * remaining
+
+    # Add up totals for future steps
+    for i in range(self.step_idx + 1, len(self.route)):
+      total_distance += self.route[i]['distance']
+      total_time += self.route[i]['duration']
+      if self.route[i]['duration_typical'] is None:
+        total_time_typical += self.route[i]['duration']
+      else:
+        total_time_typical += self.route[i]['duration_typical']
+
+    msg.navInstruction.distanceRemaining = total_distance
+    msg.navInstruction.timeRemaining = total_time
+    msg.navInstruction.timeRemainingTypical = total_time_typical
+
+    # Speed limit
+    closest_idx, closest = min(enumerate(geometry), key=lambda p: p[1].distance_to(self.last_position))
+    if closest_idx > 0:
+      # If we are not past the closest point, show previous
+      if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]):
+        closest = geometry[closest_idx - 1]
+
+    if ('maxspeed' in closest.annotations) and self.localizer_valid:
+      msg.navInstruction.speedLimit = closest.annotations['maxspeed']
+
+    # Speed limit sign type
+    if 'speedLimitSign' in step:
+      if step['speedLimitSign'] == 'mutcd':
+        msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd
+      elif step['speedLimitSign'] == 'vienna':
+        msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna
+
+    self.pm.send('navInstruction', msg)
+
+    # Transition to next route segment
+    if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD:
+      if self.step_idx + 1 < len(self.route):
+        self.step_idx += 1
+        self.reset_recompute_limits()
+      else:
+        cloudlog.warning("Destination reached")
+        print("Destination reached")
+
+        # Clear route if driving away from destination
+        dist = self.nav_destination.distance_to(self.last_position)
+        if dist > REROUTE_DISTANCE:
+          self.params.remove("NavDestination")
+          self.clear_route()
+
+  def send_route(self):
+    coords = []
+
+    if self.route is not None:
+      for path in self.route_geometry:
+        coords += [c.as_dict() for c in path]
+
+    if len(coords) == 0:
+      return
+    #print("$$$$$$$$$$$$$$$$$$coords=",coords)
+    msg = messaging.new_message('navRouteNavd', valid=True)
+    msg.navRouteNavd.coordinates = coords
+    self.pm.send('navRouteNavd', msg)
+
+  def clear_route(self):
+    self.route = None
+    self.route_geometry = None
+    self.step_idx = None
+    self.nav_destination = None
+
+  def reset_recompute_limits(self):
+    self.recompute_backoff = 0
+    self.recompute_countdown = 0
+
+  def should_recompute(self):
+    if self.step_idx is None or self.route is None:
+      return True
+
+    # Don't recompute in last segment, assume destination is reached
+    if self.step_idx == len(self.route) - 1:
+      return False
+
+    # Compute closest distance to all line segments in the current path
+    min_d = REROUTE_DISTANCE + 1
+    path = self.route_geometry[self.step_idx]
+    for i in range(len(path) - 1):
+      a = path[i]
+      b = path[i + 1]
+
+      if a.distance_to(b) < 1.0:
+        continue
+
+      min_d = min(min_d, minimum_distance(a, b, self.last_position))
+
+    if min_d > REROUTE_DISTANCE:
+      self.reroute_counter += 1
+    else:
+      self.reroute_counter = 0
+    return self.reroute_counter > REROUTE_COUNTER_MIN
+    # TODO: Check for going wrong way in segment
+
+def main():
+  pm = messaging.PubMaster(['navInstruction', 'navRouteNavd'])
+  sm = messaging.SubMaster(['liveLocationKalman', 'managerState', 'carrotMan'])
+
+  rk = Ratekeeper(1.0)
+  route_engine = RouteEngine(sm, pm)
+  while True:
+    route_engine.update()
+    rk.keep_time()
+
+
+if __name__ == "__main__":
+  main()

selfdrive/ui/carrot.cc

@@ -877,6 +877,7 @@ private:
     int xDistToTurn = 0;
     int nRoadLimitSpeed = 20;
     int active_carrot = 0;
+    bool active_navi_inst = false;
 
     int nGoPosDist = 0;
     int nGoPosTime = 0;
@@ -896,8 +897,8 @@ protected:
         if (xDistToTurn < 1500 && xDistToTurn > 0) {
             SubMaster& sm = *(s->sm);
 
-            const auto carrot_man = sm["carrotMan"].getCarrotMan();
+            //const auto carrot_man = sm["carrotMan"].getCarrotMan();
-            szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText());
+            //szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText());
 
             const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
             const auto road_edges = model.getRoadEdges();
@@ -1032,7 +1033,8 @@ protected:
         szPosRoadName = "구문천 1길 17";
 #endif
 
-        if (active_carrot <= 1) return;
+        //if (active_carrot <= 1) return;
+        //printf("nGoPosDist=%d, nGoPosTime=%d\n", nGoPosDist, nGoPosTime);
         if (nGoPosDist > 0 && nGoPosTime > 0);
 		    else return;
 
@@ -1115,14 +1117,27 @@ public:
             return;
         }
         const auto carrot_man = sm["carrotMan"].getCarrotMan();
+        const auto nav_inst = sm["navInstruction"].getNavInstruction();
         //const auto car_state = sm["carState"].getCarState();
+        active_carrot = carrot_man.getActiveCarrot();
+        //printf("active_carrot: %d\n", active_carrot);
+        if (active_carrot <= 1 || (carrot_man.getNGoPosDist() <= 0 && nav_inst.getManeuverDistance() > 0)) active_navi_inst = true;
+        else active_navi_inst = false;
+
+        if (active_carrot > 1) {
           xSpdLimit = carrot_man.getXSpdLimit();
           xSpdDist = carrot_man.getXSpdDist();
           xSignType = carrot_man.getXSpdType();
+        }
+        else {
+          xSpdLimit = 0;
+          xSpdDist = 0;
+          xSignType = 0;
+        }
+        if (active_carrot > 1 and !active_navi_inst) {
           xTurnInfo = carrot_man.getXTurnInfo();
           xDistToTurn = carrot_man.getXDistToTurn();
           nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
-        active_carrot = carrot_man.getActiveCarrot();
           atc_type = QString::fromStdString(carrot_man.getAtcType());
 
           nGoPosDist = carrot_man.getNGoPosDist();
@@ -1131,65 +1146,36 @@ public:
           szPosRoadName = QString::fromStdString(carrot_man.getSzPosRoadName());
           szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText());
           
+        } else if(sm.valid("navInstruction")) {
+          xTurnInfo = 0;
+          xDistToTurn = nav_inst.getManeuverDistance();
+          nRoadLimitSpeed = nav_inst.getSpeedLimit();
+          QString maneuverType = QString::fromStdString(nav_inst.getManeuverType());
+          QString manuverModifier = QString::fromStdString(nav_inst.getManeuverModifier());
+          if (maneuverType == "turn") {
+            if (manuverModifier == "sharp left" || manuverModifier == "slight left" || manuverModifier == "left") xTurnInfo = 1; // left turn
+            else if (manuverModifier == "sharp right" || manuverModifier == "slight right" || manuverModifier == "right") xTurnInfo = 2;
+            else if (manuverModifier == "uturn") xTurnInfo = 5;
+          }
+          else if (maneuverType == "fork" || maneuverType == "off ramp") {
+            if (manuverModifier == "slight left" || manuverModifier == "left") xTurnInfo = 3; // left turn
+            else if (manuverModifier == "slight right" || manuverModifier == "right") xTurnInfo = 4;
+          }
+
+          atc_type = maneuverType + " " + manuverModifier;
+          nGoPosDist = nav_inst.getDistanceRemaining();
+          nGoPosTime = nav_inst.getTimeRemaining();
+          szSdiDescr = "";
+          szPosRoadName = "";
+          szTBTMainText = QString::fromStdString(nav_inst.getManeuverPrimaryText());          
+        }
+
 #ifdef __UI_TEST
         active_carrot = 2;
         xSpdLimit = 110;
         xSpdDist = 12345;
         nRoadLimitSpeed = 110;
 #endif
-        if (false) {
-            int bx = s->fb_w - 120;// 350;// 150;
-            int by = 300;// s->fb_h - 150; // 410;
-            char str[128] = "";
-
-            if (xSpdLimit > 0) {
-                if (xSignType == 22) {
-                    ui_draw_image(s, { bx - 60, by - 50, 120, 150 }, "ic_speed_bump", 1.0f);
-                }
-                else {
-                    nvgBeginPath(s->vg);
-                    nvgCircle(s->vg, bx, by, 140 / 2);
-                    nvgFillColor(s->vg, COLOR_WHITE);
-                    nvgFill(s->vg);
-                    nvgBeginPath(s->vg);
-                    nvgCircle(s->vg, bx, by, 130 / 2);
-                    nvgFillColor(s->vg, COLOR_RED);
-                    nvgFill(s->vg);
-                    nvgBeginPath(s->vg);
-                    nvgCircle(s->vg, bx, by, 110 / 2);
-                    nvgFillColor(s->vg, COLOR_WHITE);
-                    nvgFill(s->vg);
-                    sprintf(str, "%d", xSpdLimit);
-                    ui_draw_text(s, bx, by + 25, str, 60, COLOR_BLACK, BOLD, 0.0f, 0.0f);
-                }
-                if (xSpdDist < 1000) sprintf(str, "%d m", xSpdDist);
-                else  sprintf(str, "%.1f km", xSpdDist / 1000.f);
-                ui_draw_text(s, bx, by + 120, str, 40, COLOR_WHITE, BOLD);
-            }
-            else if (false && xTurnInfo > 0) {
-                switch (xTurnInfo) {
-                case 1: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_turn_l", 1.0f); break;
-                case 2: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_turn_r", 1.0f); break;
-                case 3: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_lane_change_l", 1.0f); break;
-                case 4: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_lane_change_r", 1.0f); break;
-                case 7: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_turn_u", 1.0f); break;
-                case 6: ui_draw_text(s, bx, by + 20, "TG", 35, COLOR_WHITE, BOLD); break;
-                case 8: ui_draw_text(s, bx, by + 20, "arrived", 35, COLOR_WHITE, BOLD); break;
-                default:
-                    sprintf(str, "unknown(%d)", xTurnInfo);
-                    ui_draw_text(s, bx, by + 20, str, 35, COLOR_WHITE, BOLD, 0.0f, 0.0f);
-                    break;
-                }
-                if (xDistToTurn < 1000) sprintf(str, "%d m", xDistToTurn);
-                else  sprintf(str, "%.1f km", xDistToTurn / 1000.f);
-                ui_draw_text(s, bx, by + 120, str, 40, COLOR_WHITE, BOLD);
-            }
-            else if (active_carrot > 1 && nRoadLimitSpeed >= 30 && nRoadLimitSpeed < 200) {
-                ui_draw_image(s, { bx - 60, by - 50, 120, 150 }, "ic_road_speed", 1.0f);
-                sprintf(str, "%d", nRoadLimitSpeed);
-                ui_draw_text(s, bx, by + 75, str, 50, COLOR_BLACK, BOLD, 0.0f, 0.0f);
-            }
-        }
         drawTurnInfo(s);
         drawSpeedLimit(s);
         drawTurnInfoHud(s);
@@ -1887,6 +1873,7 @@ public:
     int     trafficState = 0;
     int     trafficState_carrot = 0;
     int     active_carrot = 0;
+    bool    active_navi_inst = false;
     float   xTarget = 0.0;
     int     myDrivingMode = 1;
 
@@ -1984,6 +1971,13 @@ public:
             szPosRoadName = "";
             nRoadLimitSpeed = 30;
 		}
+    const auto nav_inst = sm["navInstruction"].getNavInstruction();
+    if (active_carrot <= 1 || (carrot_man.getNGoPosDist() <= 0 && nav_inst.getManeuverDistance() > 0)) active_navi_inst = true;
+    else active_navi_inst = false;
+    if (active_navi_inst) {
+      nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
+    }
+
         xState = lp.getXState();
         trafficState = lp.getTrafficState();
         xTarget = lp.getXTarget();
@@ -2298,7 +2292,7 @@ public:
         xSignType = 1;
 #endif
 
-        if (active_carrot >= 2) {
+        if (active_carrot >= 2 || active_navi_inst) {
             dx = bx + 75;
             dy = by + 175;
             int disp_speed = 0;

selfdrive/ui/qt/maps/map.cc

@@ -279,14 +279,14 @@ void MapWindow::updateState(const UIState &s) {
     interaction_counter--;
   }
 
-  if (sm.updated("navInstruction")) {
+  if (sm.updated("navInstruction") || sm.updated("navInstructionCarrot")) {
     // an invalid navInstruction packet with a nav destination is only possible if:
     // - API exception/no internet
     // - route response is empty
     // - any time navd is waiting for recompute_countdown
     routing_problem = !sm.valid("navInstruction") && coordinate_from_param("NavDestination").has_value();
 
-    if (sm.valid("navInstruction")) {
+    if (sm.valid("navInstruction") || sm.valid("navInstructionCarrot")) {
       //auto i = sm["navInstruction"].getNavInstruction();
       //map_eta->updateETA(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining());
 

selfdrive/ui/ui.cc

@@ -104,7 +104,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
     "longitudinalPlan",
     "carControl", "carrotMan", "liveTorqueParameters", "lateralPlan", "liveParameters",
-    "navRoute", "navInstruction", "liveLocationKalman",
+    "navRoute", "navInstruction", "navInstructionCarrot", "liveLocationKalman",
   });
   prime_state = new PrimeState(this);
   language = QString::fromStdString(Params().get("LanguageSetting"));

system/manager/process_config.py

@@ -102,6 +102,7 @@ procs = [
   PythonProcess("deleter", "system.loggerd.deleter", always_run),
   PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(WEBCAM or not PC)),
   PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),
+  PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
   PythonProcess("pandad", "selfdrive.pandad.pandad", always_run),
   PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
   NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),