mapbox navi
This commit is contained in:
ajouatom
parent
9837da3936
commit
5de1dffeb7
@ -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;
|
||||
customReservedRawData2 @126 :Data;
|
||||
navRouteNavd @125 :NavRoute;
|
||||
navInstructionCarrot @126 :NavInstruction;
|
||||
|
||||
# *********** Custom: reserved for forks ***********
|
||||
carrotMan @107 :Custom.CarrotMan;
|
||||
|
||||
@ -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),
|
||||
|
||||
@ -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 < desired_speed and self.d_rel < 60:
|
||||
v_cruise_kph += 5
|
||||
#elif self.model_v_kph + 5 > v_cruise_kph and v_cruise_kph < desired_speed:
|
||||
# v_cruise_kph += 5
|
||||
if self.v_lead_kph + 5 > v_cruise_kph and v_cruise_kph < road_limit_kph and self.d_rel < 60:
|
||||
v_cruise_kph = min(v_cruise_kph + 5, road_limit_kph)
|
||||
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)
|
||||
|
||||
@ -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.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstruction"])
|
||||
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd'])
|
||||
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)
|
||||
msg.navRoute.coordinates = coords
|
||||
self.pm.send('navRoute', msg)
|
||||
#self.carrot_route_active = True
|
||||
#self.params.put_bool_nonblocking("CarrotRouteActive", True)
|
||||
self.send_routes(coords)
|
||||
"""
|
||||
try:
|
||||
module_name = "route_engine"
|
||||
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()
|
||||
|
||||
@ -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}
|
||||
|
||||
@ -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"):
|
||||
|
||||
@ -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))
|
||||
|
||||
|
||||
|
||||
@ -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 Coordinate(pos['latitude'], pos['longitude'])
|
||||
return None, None
|
||||
place_name = pos.get('place_name', None)
|
||||
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']
|
||||
|
||||
394
selfdrive/navd/navd.py
Normal file
394
selfdrive/navd/navd.py
Normal file
@ -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()
|
||||
@ -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();
|
||||
szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText());
|
||||
//const auto carrot_man = sm["carrotMan"].getCarrotMan();
|
||||
//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;
|
||||
|
||||
@ -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());
|
||||
|
||||
|
||||
@ -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"));
|
||||
|
||||
@ -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),
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user