From 0790e4d9f7ad87ea064b682fc487171dbfe0f2ae Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Sun, 5 Feb 2023 19:58:26 -0500 Subject: [PATCH] gpxd (#106) --- release/files_common | 3 + selfdrive/gpxd/gpx_uploader.py | 146 +++++++++++++++++++++++ selfdrive/gpxd/gpxd.py | 174 ++++++++++++++++++++++++++++ selfdrive/manager/process_config.py | 3 + 4 files changed, 326 insertions(+) create mode 100644 selfdrive/gpxd/gpx_uploader.py create mode 100644 selfdrive/gpxd/gpxd.py diff --git a/release/files_common b/release/files_common index 5b310d5c80..644a9c34b7 100644 --- a/release/files_common +++ b/release/files_common @@ -143,6 +143,9 @@ selfdrive/debug/uiview.py selfdrive/debug/hyundai_enable_radar_points.py selfdrive/debug/vw_mqb_config.py +selfdrive/gpxd/gpx_uploader.py +selfdrive/gpxd/gpxd.py + common/SConscript common/version.h diff --git a/selfdrive/gpxd/gpx_uploader.py b/selfdrive/gpxd/gpx_uploader.py new file mode 100644 index 0000000000..4c6e57aa8e --- /dev/null +++ b/selfdrive/gpxd/gpx_uploader.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 +# The MIT License +# +# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import os +import time +from common.params import Params +from common.realtime import Ratekeeper +from system.version import get_version + +# for uploader +from selfdrive.loggerd.xattr_cache import getxattr, setxattr +import glob +import requests +import json + +RATE = 1 + +# customisable values +GPX_LOG_PATH = '/data/media/0/gpx_logs/' +LOG_HERTZ = 1/10 # 0.1 Hz = 10 sec, higher for higher accuracy, 10hz seems fine + +# uploader +UPLOAD_ATTR_NAME = 'user.upload' +UPLOAD_ATTR_VALUE = b'1' +LOG_PATH = '/data/media/0/gpx_logs/' + +# osm api +API_HEADER = {'Authorization': 'Bearer DjlXVi1kL9bQ4GBj_3ixiKufVXzsHhpYCTi3ayNYw7k'} +VERSION_URL = 'https://api.openstreetmap.org/api/versions' +UPLOAD_URL = 'https://api.openstreetmap.org/api/0.6/gpx/create' + +_DEBUG = False + + +def _debug(msg): + if not _DEBUG: + return + print(msg, flush=True) + + +class GpxUploader(): + def __init__(self): + self.param_s = Params() + self.rk = Ratekeeper(RATE, print_delay_threshold=None) + + self._delete_after_upload = self.param_s.get_bool('GpxDeleteAfterUpload') + self._car_model = "Unknown Model" + # read model from LiveParameters + params = self.param_s.get("LiveParameters") + if params is not None: + params = json.loads(params) + self._car_model = params.get('carFingerprint', self._car_model) + self._sp_version = get_version() + _debug("GpxUploader init - _delete_after_upload = %s" % self._delete_after_upload) + _debug("GpxUploader init - _car_model = %s" % self._car_model) + + def update(self): + while True: + files = self._get_files_to_be_uploaded() + if len(files) == 0 or not self._is_online(): + _debug("run - not online or no files") + elif not self.param_s.get_bool("DisableOnroadUploads") or self.param_s.get_bool("IsOffroad"): + for file in files: + if self._do_upload(file): + if self._delete_after_upload: + _debug("run - _delete_after_upload") + os.remove(file) + else: + _debug("run - set_is_uploaded") + self._set_is_uploaded(file) + # sleep for 300 secs if offroad + # otherwise sleep 60 secs + time.sleep(300 if self.param_s.get_bool("IsOffroad") else 60) + self.rk.keep_time() + + def _is_online(self): + try: + r = requests.get(VERSION_URL, headers=API_HEADER) + _debug("is_online? status_code = %s" % r.status_code) + return r.status_code >= 200 + except: + return False + + def _get_is_uploaded(self, filename): + _debug("%s is uploaded: %s" % (filename, getxattr(filename, UPLOAD_ATTR_NAME) is not None)) + return getxattr(filename, UPLOAD_ATTR_NAME) is not None + + def _set_is_uploaded(self, filename): + _debug("%s set to uploaded" % filename) + setxattr(filename, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) + + def _get_files(self): + return sorted( filter( os.path.isfile, glob.glob(LOG_PATH + '*') ) ) + + def _get_files_to_be_uploaded(self): + files = self._get_files() + files_to_be_uploaded = [] + for file in files: + if not self._get_is_uploaded(file): + files_to_be_uploaded.append(file) + return files_to_be_uploaded + + def _do_upload(self, filename): + fn = os.path.basename(filename) + data = { + 'description': "Routes from sunnypilot %s (%s)." % (self._sp_version, self._car_model), + 'visibility': 'identifiable' + } + files = { + "file": (fn, open(filename, 'rb')) + } + try: + r = requests.post(UPLOAD_URL, files=files, data=data, headers=API_HEADER) + _debug("do_upload - %s - %s" % (filename, r.status_code)) + return r.status_code == 200 + except: + return False + + +def main(): + gpx_uploader = GpxUploader() + gpx_uploader.update() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/gpxd/gpxd.py b/selfdrive/gpxd/gpxd.py new file mode 100644 index 0000000000..d1e1810b4a --- /dev/null +++ b/selfdrive/gpxd/gpxd.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +# The MIT License +# +# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import cereal.messaging as messaging +import os +import datetime +import signal +import threading +from common.realtime import Ratekeeper +from cereal import log +from system.swaglog import cloudlog + +# customisable values +GPX_LOG_PATH = '/data/media/0/gpx_logs/' +LOG_HERTZ = 10 # 10 hz = 0.1 sec, higher for higher accuracy, 10hz seems fine +LOG_LENGTH = 10 # mins, higher means it keeps more data in the memory, will take more time to write into a file too. +LOST_SIGNAL_COUNT_LENGTH = 10 # secs, output log file if we lost signal for this long + +# do not change +LOST_SIGNAL_COUNT_MAX = LOST_SIGNAL_COUNT_LENGTH * LOG_HERTZ # secs, +LOGS_PER_FILE = LOG_LENGTH * 60 * LOG_HERTZ # e.g. 10 * 60 * 10 = 6000 points per file + + +_DEBUG = False +_CLOUDLOG_DEBUG = True + + +def _debug(msg, log_to_cloud=True): + if _CLOUDLOG_DEBUG and log_to_cloud: + cloudlog.debug(msg) + if _DEBUG: + print(msg) + + +class WaitTimeHelper: + ready_event = threading.Event() + shutdown = False + + def __init__(self): + signal.signal(signal.SIGTERM, self.graceful_shutdown) + signal.signal(signal.SIGINT, self.graceful_shutdown) + signal.signal(signal.SIGHUP, self.graceful_shutdown) + + def graceful_shutdown(self, signum, frame): + self.shutdown = True + self.ready_event.set() + + +class GpxD: + def __init__(self, sm): + self.sm = sm + self.rk = Ratekeeper(LOG_HERTZ, print_delay_threshold=None) + + self.log_count = 0 + self.logs = list() + self.lost_signal_count = 0 + self.wait_helper = WaitTimeHelper() + self.started_time = datetime.datetime.utcnow().isoformat() + self.v_ego_prev = 0. + self.pause = True + + def log(self): + location = self.sm['liveLocationKalman'] + gps = self.sm['gpsLocationExternal'] + v_ego = self.sm['carState'].vEgo + + if abs(v_ego) > 0.: + self.pause = False + + location_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + _debug("gpxd: location_valid - %s" % location_valid) + if not location_valid or self.pause: + if self.log_count > 0: + self.lost_signal_count += 1 + else: + lat = location.positionGeodetic.value[0] + lon = location.positionGeodetic.value[1] + alt = gps.altitude + + _debug("gpxd: logged - %s %s %s %s" % (datetime.datetime.utcfromtimestamp(location.unixTimestampMillis*0.001).isoformat(), str(lat), str(lon), str(alt))) + self.logs.append([datetime.datetime.utcfromtimestamp(location.unixTimestampMillis*0.001).isoformat(), str(lat), str(lon), str(alt)]) + self.log_count += 1 + self.lost_signal_count = 0 + + if not self.pause and abs(v_ego) < 0.01: + _debug("gpxd: paused") + self.pause = True + + self.v_ego_prev = v_ego + + def write_log(self, force = False): + if self.log_count == 0: + return + + if force or (self.log_count >= LOGS_PER_FILE or self.lost_signal_count >= LOST_SIGNAL_COUNT_MAX): + _debug("gpxd: save to log") + self._write_gpx() + self.lost_signal_count = 0 + self.log_count = 0 + self.logs.clear() + self.started_time = datetime.datetime.utcnow().isoformat() + + def _write_gpx(self): + if len(self.logs) > 1: + if not os.path.exists(GPX_LOG_PATH): + os.makedirs(GPX_LOG_PATH) + filename = self.started_time.replace(':','-') + str = '' + str += "\n" + str += "\n" + str += "\n" + str += " " + self.started_time + "" + str += " \n" + for trkpt in self.logs: + str += self._trkpt_template(trkpt[1], trkpt[2], trkpt[3], trkpt[0]) + str += " \n" + str += "\n" + str += "\n" + try: + f = open('%s%sZ.gpx' % (GPX_LOG_PATH, filename), 'w') + f.write(str) + f.close() + except: + pass + + def _trkpt_template(self, lat, lon, ele, time): + str = "" + str += " \n" + str += " " + ele + "\n" + str += " \n" + str += " \n" + return str + + def gpxd_thread(self): + while True: + self.sm.update(0) + self.log() + self.write_log() + if self.wait_helper.shutdown: + self.write_log(True) + break + self.rk.keep_time() + + +def main(sm=None): + if sm is None: + sm = messaging.SubMaster(['liveLocationKalman', 'gpsLocationExternal', 'carState']) + + gpx = GpxD(sm) + gpx.gpxd_thread() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 8fc4d94e55..7fbc25fe4d 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -64,6 +64,9 @@ procs = [ PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True), PythonProcess("statsd", "selfdrive.statsd", offroad=True), + PythonProcess("gpxd", "selfdrive.gpxd.gpxd"), + PythonProcess("gpxd_uploader", "selfdrive.gpxd.gpx_uploader", offroad=True), + # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar), PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar),