def calculate_route(self, destination):
cloudlog.warning(f"Calculating route {self.last_position} -> {destination}")
self.nav_destination = destination
params = {
'access_token': self.mapbox_token,
'annotations': 'maxspeed',
'geometries': 'geojson',
'overview': 'full',
'steps': 'true',
'banner_instructions': 'true',
'alternatives': 'false',
}
if self.last_bearing is not None:
params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;"
url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}'
try:
resp = requests.get(url, params=params)
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)) and ('unknown' not in maxspeeds[maxspeed_idx]):
coord.annotations['maxspeed'] = maxspeed_to_ms(maxspeeds[maxspeed_idx])
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")
self.clear_route()
except requests.exceptions.RequestException:
cloudlog.exception("failed to get route")
self.clear_route()
self.send_route()
def update_location(self):
location = self.sm['liveLocationKalman']
self.gps_ok = location.gpsOK
localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
if localizer_valid:
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
def update_location(self):
location = self.sm['liveLocationKalman']
laikad = self.sm['gnssMeasurements']
locationd_valid = (location.status
== log.LiveLocationKalman.Status.valid
) and location.positionGeodetic.valid
laikad_valid = laikad.positionECEF.valid and np.linalg.norm(
laikad.positionECEF.std) < VALID_POS_STD
self.localizer_valid = locationd_valid or laikad_valid
self.gps_ok = location.gpsOK or laikad_valid
if locationd_valid:
self.last_bearing = math.degrees(
location.calibratedOrientationNED.value[2])
self.last_position = Coordinate(location.positionGeodetic.value[0],
location.positionGeodetic.value[1])
elif laikad_valid:
geodetic = ecef2geodetic(laikad.positionECEF.value)
self.last_position = Coordinate(geodetic[0], geodetic[1])
self.last_bearing = None
def calculate_route(self, destination):
cloudlog.warning(
f"Calculating route {self.last_position} -> {destination}")
self.nav_destination = destination
params = {
'access_token': self.mapbox_token,
# 'annotations': 'maxspeed',
'geometries': 'geojson',
'overview': 'full',
'steps': 'true',
'banner_instructions': 'true',
'alternatives': 'false',
}
if self.last_bearing is not None:
params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;"
url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}'
try:
resp = requests.get(url, params=params)
resp.raise_for_status()
r = resp.json()
if len(r['routes']):
self.route = r['routes'][0]['legs'][0]['steps']
self.route_geometry = []
# Convert coordinates
for step in self.route:
self.route_geometry.append([
Coordinate.from_mapbox_tuple(c)
for c in step['geometry']['coordinates']
])
self.step_idx = 0
else:
cloudlog.warning("Got empty route response")
self.clear_route()
except requests.exceptions.RequestException:
cloudlog.exception("failed to get route")
self.clear_route()
self.send_route()