diff --git a/.github/PULL_REQUEST_TEMPLATE/python.md b/.github/PULL_REQUEST_TEMPLATE/python.md index df5c4daa9..434c16509 100644 --- a/.github/PULL_REQUEST_TEMPLATE/python.md +++ b/.github/PULL_REQUEST_TEMPLATE/python.md @@ -52,7 +52,7 @@ in the python venv._ 4. in another terminal, start a fake _gpsd_ process (no need for an actual GNSS device): ```sh - $ TMPDIR=/tmp gpsfake -q -P 2947 -u -n -c 0.1 tests/data/NMEA.log + $ TMPDIR=/tmp gpsfake -q -P 2948 -u -n -c 0.1 tests/data/NMEA.log ``` 5. in another terminal, start a container with Python 3.11 and the necessary packages: @@ -69,7 +69,7 @@ in the python venv._ --user $(id -u):$(id -u) \ --mount type=bind,source=$(pwd),destination=$(pwd) \ --workdir $(pwd) \ - python:3.11.17-slim-trixie \ + python:3.11.15-slim-trixie \ /bin/bash -il $ docker container exec -u 0:0 iot3 apt update diff --git a/README.md b/README.md index b2e9ee027..1e3cb262b 100644 --- a/README.md +++ b/README.md @@ -97,7 +97,7 @@ they are using the following versions: | Schema | Rust | Python | Java | Swift | |:-----------------:|:-----------------------------------------------------------------------------------:|:---------------------------------------------------------:|:-----------------------------------------------------------------------------------------------------------------------------------:|:-------------------------------------------:| | **Bootstrap** | | | | | -| **CAM** | [2.4.0](schema/cam/cam_schema_2-4-0.json) [1.1.3](schema/cam/cam_schema_1-1-3.json) | [1.1.3](schema/cam/cam_schema_1-1-3.json) | [2.4.0](schema/cam/cam_schema_2-4-0.json) [1.1.3](schema/cam/cam_schema_1-1-3.json) | [1.1.3](schema/cam/cam_schema_1-1-3.json) | +| **CAM** | [2.4.0](schema/cam/cam_schema_2-4-0.json) [1.1.3](schema/cam/cam_schema_1-1-3.json) | [2.4.0](schema/cam/cam_schema_2-4-0.json) | [2.4.0](schema/cam/cam_schema_2-4-0.json) [1.1.3](schema/cam/cam_schema_1-1-3.json) | [1.1.3](schema/cam/cam_schema_1-1-3.json) | | **CPM** | [2.1.1](schema/cpm/cpm_schema_2-1-1.json) | [2.1.1](schema/cpm/cpm_schema_2-1-1.json) | [2.1.1](schema/cpm/cpm_schema_2-1-1.json) [1.2.1](schema/cpm/cpm_schema_1-2-1.json) | | | **DENM** | [2.2.0](schema/denm/denm_schema_2-2-0.json) | [1.1.3](schema/denm/denm_schema_1-1-3.json) | [2.3.0](schema/denm/denm_schema_2-3-0.json) [2.2.0](schema/denm/denm_schema_2-2-0.json) [1.1.3](schema/denm/denm_schema_1-1-3.json) | [1.1.3](schema/denm/denm_schema_1-1-3.json) | | **Information** | [2.1.0](schema/information/information_schema_2-1-0.json) | [1.2.0](schema/information/information_schema_1-2-0.json) | | | diff --git a/python/iot3/src/iot3/mobility/cam.py b/python/iot3/src/iot3/mobility/cam.py index 4df727943..da6e5d902 100644 --- a/python/iot3/src/iot3/mobility/cam.py +++ b/python/iot3/src/iot3/mobility/cam.py @@ -36,9 +36,8 @@ def __init__( self._message = dict( { - "type": "cam", - "origin": "self", - "version": "1.1.3", + "message_type": "cam", + "message_format": "json/raw", "source_uuid": uuid, "timestamp": ( etsi.ETSI.si2etsi( @@ -47,6 +46,7 @@ def __init__( 0, ) ), + "version": "2.4.0", "message": { "protocol_version": 1, "station_id": self.station_id(uuid), @@ -55,68 +55,51 @@ def __init__( ), "basic_container": { "station_type": station_type, - "reference_position": { - "latitude": etsi.ETSI.si2etsi( - gnss_report.latitude, - etsi.ETSI.DECI_MICRO_DEGREE, - 900000001, - ), - "longitude": etsi.ETSI.si2etsi( - gnss_report.longitude, - etsi.ETSI.DECI_MICRO_DEGREE, - 1800000001, - ), - "altitude": etsi.ETSI.si2etsi( - gnss_report.altitude, - etsi.ETSI.CENTI_METER, - 800001, - ), - }, - "confidence": { - "position_confidence_ellipse": { - # We treat the 2D error as a circle, so semi-major - # and semi-minor are eqal, and thus the orientation - # of the elipse does not matter. - "semi_major_confidence": etsi.ETSI.si2etsi( - gnss_report.horizontal_error, - etsi.ETSI.CENTI_METER, - 4095, - {"min": 0, "max": 4093}, - 4094, - ), - "semi_minor_confidence": etsi.ETSI.si2etsi( - gnss_report.horizontal_error, - etsi.ETSI.CENTI_METER, - 4095, - {"min": 0, "max": 4093}, - 4094, - ), - # Any orientation is valid for a circle, just use 0. - "semi_major_orientation": etsi.ETSI.si2etsi( - 0, + "reference_position": self.reference_position(gnss_report), + }, + "high_frequency_container": { + "basic_vehicle_container_high_frequency": { + "heading": { + "value": etsi.ETSI.si2etsi( + gnss_report.track, etsi.ETSI.DECI_DEGREE, 3601, ), + "confidence": 127, + }, + "speed": { + "value": etsi.ETSI.si2etsi( + gnss_report.speed, + etsi.ETSI.CENTI_METER_PER_SECOND, + 16383, + ), + "confidence": 127, + }, + "drive_direction": 2, + "vehicle_length": { + "value": 1023, + "confidence": 4, + }, + "vehicle_width": 62, + "longitudinal_acceleration": { + "value": etsi.ETSI.si2etsi( + gnss_report.acceleration, + etsi.ETSI.DECI_METER_PER_SECOND_SECOND, + 161, + ), + "confidence": 102, + }, + "curvature": { + "value": 1023, + "confidence": 7, + }, + "curvature_calculation_mode": 2, + "yaw_rate": { + "value": 32767, + "confidence": 8, }, }, }, - "high_frequency_container": { - "heading": etsi.ETSI.si2etsi( - gnss_report.track, - etsi.ETSI.DECI_DEGREE, - 3601, - ), - "speed": etsi.ETSI.si2etsi( - gnss_report.speed, - etsi.ETSI.CENTI_METER_PER_SECOND, - 16383, - ), - "longitudinal_acceleration": etsi.ETSI.si2etsi( - gnss_report.acceleration, - etsi.ETSI.DECI_METER_PER_SECOND_SECOND, - 161, - ), - }, }, }, ) @@ -166,15 +149,15 @@ def altitude(self): return etsi.ETSI.etsi2si( self._message["message"]["basic_container"]["reference_position"][ "altitude" - ], + ]["value"], etsi.ETSI.CENTI_METER, 800001, ) @altitude.setter def altitude(self, altitude): - self._message["message"]["basic_container"]["reference_position"][ - "altitude" + self._message["message"]["basic_container"]["reference_position"]["altitude"][ + "value" ] = etsi.ETSI.si2etsi( altitude, etsi.ETSI.CENTI_METER, diff --git a/python/iot3/src/iot3/mobility/cpm.py b/python/iot3/src/iot3/mobility/cpm.py index 747570c69..9db16b2d9 100644 --- a/python/iot3/src/iot3/mobility/cpm.py +++ b/python/iot3/src/iot3/mobility/cpm.py @@ -155,57 +155,7 @@ def __init__( "management_container": { "station_type": station_type, "reference_time": etsi.ETSI.unix2etsi_time(self._timestamp), - "reference_position": { - "latitude": etsi.ETSI.si2etsi( - gnss_report.latitude, - etsi.ETSI.DECI_MICRO_DEGREE, - 900000001, - ), - "longitude": etsi.ETSI.si2etsi( - gnss_report.longitude, - etsi.ETSI.DECI_MICRO_DEGREE, - 1800000001, - ), - "altitude": { - "value": etsi.ETSI.si2etsi( - gnss_report.altitude, - etsi.ETSI.CENTI_METER, - 800001, - ), - # Encoding the altitude error is a non-linear search in - # an array... Let's consider it unavailable for now. - "confidence": etsi.ETSI.si2etsi( - None, - etsi.ETSI.CENTI_METER, - 15, - ), - }, - "position_confidence_ellipse": { - # We treat the 2D error as a circle, so semi-major - # and semi-minor are eqal, and thus the orientation - # of the elipse does not matter. - "semi_major": etsi.ETSI.si2etsi( - gnss_report.horizontal_error, - etsi.ETSI.CENTI_METER, - 4095, - {"min": 0, "max": 4093}, - 4094, - ), - "semi_minor": etsi.ETSI.si2etsi( - gnss_report.horizontal_error, - etsi.ETSI.CENTI_METER, - 4095, - {"min": 0, "max": 4093}, - 4094, - ), - # Any orientation is valid for a circle, just use 0. - "semi_major_orientation": etsi.ETSI.si2etsi( - 0, - etsi.ETSI.DECI_DEGREE, - 3601, - ), - }, - }, + "reference_position": self.reference_position(gnss_report), }, "perceived_object_container": [], }, diff --git a/python/iot3/src/iot3/mobility/etsi.py b/python/iot3/src/iot3/mobility/etsi.py index 02000474b..6c2a34c87 100644 --- a/python/iot3/src/iot3/mobility/etsi.py +++ b/python/iot3/src/iot3/mobility/etsi.py @@ -11,6 +11,7 @@ import json from typing import Optional from . import leapseconds +from .gnss import GNSSReport class ETSI(abc.ABC): @@ -191,7 +192,9 @@ def unix2etsi_time( 94694401000 """ tai_time = leapseconds.utc_to_tai( - datetime.datetime.utcfromtimestamp(unix_time) + datetime.datetime.fromtimestamp(unix_time, datetime.timezone.utc).replace( + tzinfo=None + ) ).timestamp() return ETSI.si2etsi(tai_time - ETSI.EPOCH, ETSI.MILLI_SECOND, 0) @@ -218,7 +221,9 @@ def etsi2unix_time( """ tai_time = ETSI.etsi2si(etsi_time, ETSI.MILLI_SECOND, 0) + ETSI.EPOCH return leapseconds.tai_to_utc( - datetime.datetime.utcfromtimestamp(tai_time) + datetime.datetime.fromtimestamp(tai_time, datetime.timezone.utc).replace( + tzinfo=None + ) ).timestamp() @staticmethod @@ -337,3 +342,76 @@ def topic( def to_json(self) -> str: # Return the densest-possible JSON sentence return json.dumps(self._message, separators=(",", ":")) + + @staticmethod + def reference_position( + gnss_report: GNSSReport, + ) -> dict: + return { + "latitude": ETSI.si2etsi( + gnss_report.latitude, + ETSI.DECI_MICRO_DEGREE, + 900000001, + ), + "longitude": ETSI.si2etsi( + gnss_report.longitude, + ETSI.DECI_MICRO_DEGREE, + 1800000001, + ), + "altitude": { + "value": ETSI.si2etsi( + gnss_report.altitude, + ETSI.CENTI_METER, + 800001, + ), + "confidence": Message.altitude_confidence(gnss_report), + }, + "position_confidence_ellipse": ( + Message.position_confidence_ellipse(gnss_report) + ), + } + + @staticmethod + def position_confidence_ellipse( + gnss_report: GNSSReport, + ) -> dict: + return { + "semi_major": ETSI.si2etsi( + gnss_report.ellipse_semi_major, + ETSI.CENTI_METER, + 4095, + {"min": 1, "max": 4093}, + 4094, + ), + "semi_minor": ETSI.si2etsi( + gnss_report.ellipse_semi_minor, + ETSI.CENTI_METER, + 4095, + {"min": 1, "max": 4093}, + 4094, + ), + "semi_major_orientation": ETSI.si2etsi( + ( + None + if gnss_report.ellipse_orient is None + else gnss_report.ellipse_orient % 360.0 + ), + ETSI.DECI_DEGREE, + 3601, + {"min": 0, "max": 3599}, + ), + } + + @staticmethod + def altitude_confidence( + gnss_report: GNSSReport, + ) -> int: + """Return the altitude confidence value.""" + if gnss_report.altitude_error is None: + return 15 + steps = [1, 2, 5] + for confidence in range(14): + threshold = steps[confidence % 3] * (10 ** (int(confidence / 3) - 2)) + if gnss_report.altitude_error <= threshold: + return confidence + return 14 diff --git a/python/iot3/src/iot3/mobility/gnss.py b/python/iot3/src/iot3/mobility/gnss.py index c9679bfff..1b31a14bc 100644 --- a/python/iot3/src/iot3/mobility/gnss.py +++ b/python/iot3/src/iot3/mobility/gnss.py @@ -24,6 +24,9 @@ class GNSSReport: When a field exist in both radians and degrees, only one may be set when instantiating the class, not both. The other will automatically be set. + If the error ellipse is not known, it will be emulated with the horizontal + error, if that is known. + :param timestamp: UNIX timestamp this object was created at, with arbitrary sub-second precision; this must _not_ be specified when creating a GNSSReport @@ -46,6 +49,10 @@ class GNSSReport: above), in radians :param magnetic_heading: Magnetic heading, in degrees :param magnetic_heading_r: Magnetic heading, in radians + :param ellipse_semi_major: Length of the error ellipse semi-major axis + :param ellipse_semi_minor: Length of the error ellipse semi-minor axis + :param ellipse_orient: Orientation of semi-major axis of error ellipse, + in degrees from true North. """ timestamp: float = None @@ -64,13 +71,16 @@ class GNSSReport: magnetic_heading: float | None = None true_heading_r: float | None = None magnetic_heading_r: float | None = None + ellipse_semi_major: float | None = None + ellipse_semi_minor: float | None = None + ellipse_orient: float | None = None # Frozen dataclasses do not allow directly setting their attributes, # neither directly with dot notation nor with setattr(), so we must # use the root class 'object' to set the attributes: # https://docs.python.org/3/library/dataclasses.html#frozen-instances def __post_init__(self): - if getattr(self, "timestamp") is not None: + if self.timestamp is not None: raise AttributeError( "Assigning timestamp is not allowed", name="timestamp", @@ -78,9 +88,39 @@ def __post_init__(self): ) object.__setattr__(self, "timestamp", time.time()) + # Sanitise and/or emulate the error ellipse + match self.ellipse_semi_major, self.ellipse_semi_minor, self.horizontal_error: + # No error value, no orientation + case None, None, None: + object.__setattr__(self, "ellipse_orient", None) + # If no major and no minor, use horizontal error if provided + case None, None, float(h_error): + object.__setattr__(self, "ellipse_semi_major", h_error) + object.__setattr__(self, "ellipse_semi_minor", h_error) + object.__setattr__(self, "ellipse_orient", 0.0) + # If major but no minor, use major as minor + case float(s_major), None, _: + object.__setattr__(self, "ellipse_semi_minor", s_major) + object.__setattr__(self, "ellipse_orient", 0.0) + # If minor but no major, it does not make sense; no ellipse + case None, float(_), _: + object.__setattr__(self, "ellipse_semi_minor", None) + object.__setattr__(self, "ellipse_orient", None) + # If both major and minor, check major >= minor + case float(s_major), float(s_minor), _: + if s_major < s_minor: + object.__setattr__(self, "ellipse_semi_major", s_minor) + object.__setattr__(self, "ellipse_semi_minor", s_major) + if self.ellipse_orient is not None: + object.__setattr__( + self, + "ellipse_orient", + (self.ellipse_orient + 90) % 360.0, + ) + fields = { # min_inc, max_inc: inclusive boundaries - # min_exc, max_exc: exclusibe boundaries + # min_exc, max_exc: exclusive boundaries "latitude": { "min_inc": -90.0, "max_inc": 90.0, @@ -164,25 +204,31 @@ def __init__( *, host: Optional[str] = None, port: Optional[int] = None, + persistence: Optional[float] = 1.0, ): """Simple abstraction to a gpsd daemon. :param host: The hostname or IP address the gpsd daemon runs on; by default, 127.0.0.1 :param port: The TCP port the gpsd daemon listen on; by default 2947 + :parama persistence: The duration after which the last measurement is + considered valid; afterward, no measurement will + be returned when calling get(). Both host and port are optional, as the usual setup is to have gpsd run on the local machine, and listen on its well-known port. """ self._host = host or "127.0.0.1" self._port = port or 2947 + self._persistence = persistence self._thread = threading.Thread( target=self._loop, name=f"{__name__}.gpsd_client", daemon=True, ) - self._last = dict() + self._full_epoch = {} + self._current_epoch = {} self._sock = None self._should_stop = False @@ -191,25 +237,38 @@ def start(self): def stop(self): self._should_stop = True + self._disconnect() def join(self, timeout: Optional[float] = None): self._thread.join(timeout) - def __call__(self): - last = copy.deepcopy(self._last) + def __call__( + self, + *, + max_age: Optional[float] = None, + ) -> GNSSReport | None: + """Returns a GNSSReport() object with the last valid measurement, None otherwise. + + :param max_age: The maximum age, in seconds, to consider a measurement valid; + overrides the persistence from the constructor. + """ + + epoch = copy.deepcopy(self._full_epoch) try: - tpv = last["tpv"] + tpv_data = epoch["tpv"] except (TypeError, KeyError): # No measurement yet return None now = time.time() - if now - last["tpv"]["timestamp"] > 1.0: + if now - tpv_data["timestamp"] > ( + max_age if max_age is not None else self._persistence + ): # Last measurement too old return None - tpv = json.loads(tpv["msg"]) + tpv = tpv_data["msg"] if "lat" not in tpv or "lon" not in tpv: # No latitude or no longitude return None @@ -226,7 +285,7 @@ def __call__(self): params["altitude_error"] = tpv.get("epv") try: - att = last["att"] + att = epoch["att"]["msg"] except KeyError: # Not all GNSS devices provide attitude data pass @@ -235,6 +294,16 @@ def __call__(self): params["true_heading"] = att.get("heading") params["magnetic_heading"] = att.get("mheading") + try: + gst = epoch["gst"]["msg"] + except KeyError: + # Not all GNSS devices provide pseudorange noise report data + pass + else: + params["ellipse_semi_major"] = gst.get("major") + params["ellipse_semi_minor"] = gst.get("minor") + params["ellipse_orient"] = gst.get("orient") + return GNSSReport(**params) def _connect(self): @@ -258,6 +327,7 @@ def _disconnect(self): except: # already closed, we don't care pass + self._sock_fd = None self._sock = None def _loop(self): @@ -307,11 +377,33 @@ def _loop(self): msg_class = msg["class"].lower() except KeyError: continue - if msg_class in ["tpv", "att"]: + # TPV, GST, ATT messages (and others) are emitted as separate + # json sentences, but they are usually correlated (ATT is + # explicitly documented to be "synchronous to the GNSS epoch". + # However, we don't know beforehand 1. in which order they will + # be emitted, and 2. if they will be emitted at all. There is a + # 'time' field documented for all those messages, but it may be + # missing, or its value may be way off (the documentation says: + # "May be absent if the mode is not 2D or 3D. May be present, + # but invalid, if there is no fix. Verify 3 consecutive 3D fixes + # before believing it is UTC. Even then it may be off by several + # seconds until the current leap seconds is known"). So, we + # can't rely on that field to aggregate correlated messages. + # + # So, we use a crude heuristic: we assume that the TPV + # message is the last to be emitted in a GNSS epoch, so we + # store all messages we receive, and when we get a TPV one, + # we bundle everything we have about this epoch, queue it + # for further computations, and drop all the stored messages + # to start a new epoch afresh. + if msg_class in ["tpv", "att", "gst"]: # Only store those messages we need - self._last[msg_class] = { + self._current_epoch[msg_class] = { "timestamp": time.time(), - "msg": msg_json, + "msg": msg, } + if msg_class == "tpv": + self._full_epoch = self._current_epoch + self._current_epoch = {} self._disconnect() diff --git a/python/iot3/tests/test-iot3-mobility-gnss b/python/iot3/tests/test-iot3-mobility-gnss index deb732d1e..4d039226f 100755 --- a/python/iot3/tests/test-iot3-mobility-gnss +++ b/python/iot3/tests/test-iot3-mobility-gnss @@ -2,11 +2,23 @@ import math import time +from iot3.mobility.etsi import Message from iot3.mobility.gnss import GNSSReport, GNSS +#----------------------------------------------------------------------------------------- print("Basic, empty GNSS report...") gnss_report = GNSSReport() +print("GNSSReport with GST fields...") +report = GNSSReport( + ellipse_semi_major=0.5, + ellipse_semi_minor=0.3, + ellipse_orient=90.0, +) +assert report.ellipse_semi_major == 0.5 +assert report.ellipse_semi_minor == 0.3 +assert report.ellipse_orient == 90.0 + print("Disallow explicit timestamp...") try: gnss_report = GNSSReport( @@ -25,7 +37,7 @@ try: except AttributeError: pass else: - raise RuntimeError("Out-of-range latitude should not be alowed") + raise RuntimeError("Out-of-range latitude should not be allowed") print("Conflicting attributes...") try: @@ -56,15 +68,164 @@ gnss_report = GNSSReport( if math.fabs(gnss_report.latitude - math.degrees(0.761578119)) >= 5 * 10 ** -8: raise RuntimeError("Degree -> radian latitude conversion failed") -print("gpsd connection to 127.0.0.1:2947...") +#----------------------------------------------------------------------------------------- +position_confidence_ellipse = Message.position_confidence_ellipse + +print("position_confidence_ellipse with no data...") +ellipse = position_confidence_ellipse(GNSSReport()) +assert ellipse["semi_major"] == 4095 +assert ellipse["semi_minor"] == 4095 +assert ellipse["semi_major_orientation"] == 3601 + +print("position_confidence_ellipse with GST data...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=1.5, + ellipse_semi_minor=0.8, + ellipse_orient=45.0, + ), +) +assert ellipse["semi_major"] == 150 +assert ellipse["semi_minor"] == 80 +assert ellipse["semi_major_orientation"] == 450 + +print("position_confidence_ellipse with GST major only...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=1.0, + ), +) +assert ellipse["semi_major"] == 100 +assert ellipse["semi_minor"] == 100 +assert ellipse["semi_major_orientation"] == 0 + +print("position_confidence_ellipse with GST minor only...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_minor=0.5, + ), +) +assert ellipse["semi_major"] == 4095 +assert ellipse["semi_minor"] == 4095 +assert ellipse["semi_major_orientation"] == 3601 + +print("position_confidence_ellipse with GST orient only...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_orient=90.0, + ), +) +assert ellipse["semi_major"] == 4095 +assert ellipse["semi_minor"] == 4095 +assert ellipse["semi_major_orientation"] == 3601 + +print("position_confidence_ellipse with GST major+minor, no orient...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=1.2, + ellipse_semi_minor=0.6, + ), +) +assert ellipse["semi_major"] == 120 +assert ellipse["semi_minor"] == 60 +assert ellipse["semi_major_orientation"] == 3601 + +print("position_confidence_ellipse with GST major+orient, no minor...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=2.0, + ellipse_orient=30.0, + ), +) +assert ellipse["semi_major"] == 200 +assert ellipse["semi_minor"] == 200 +assert ellipse["semi_major_orientation"] == 0 + +print("position_confidence_ellipse with GST minor+orient, no major...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_minor=0.3, + ellipse_orient=45.0, + ), +) +assert ellipse["semi_major"] == 4095 +assert ellipse["semi_minor"] == 4095 +assert ellipse["semi_major_orientation"] == 3601 + +print("position_confidence_ellipse: GST major takes priority over horizontal_error...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=1.0, + ellipse_semi_minor=0.5, + horizontal_error=9.99, + ), +) +assert ellipse["semi_major"] == 100 +assert ellipse["semi_minor"] == 50 +assert ellipse["semi_major_orientation"] == 3601 + +print("position_confidence_ellipse: horizontal_error fallback when no major...") +ellipse = position_confidence_ellipse( + GNSSReport( + horizontal_error=5.0, + ), +) +assert ellipse["semi_major"] == 500 +assert ellipse["semi_minor"] == 500 +assert ellipse["semi_major_orientation"] == 0 + +print("position_confidence_ellipse: horizontal_error ignored when major present...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=0.5, + horizontal_error=9.99, + ), +) +assert ellipse["semi_major"] == 50 +assert ellipse["semi_minor"] == 50 +assert ellipse["semi_major_orientation"] == 0 + +print("position_confidence_ellipse: orientation wrapping (360° → 0°)...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=2.0, + ellipse_semi_minor=1.0, + ellipse_orient=360.0, + ), +) +print(f"orient={ellipse['semi_major_orientation']}") +assert ellipse["semi_major_orientation"] == 0 + +print("position_confidence_ellipse: orientation wrapping (>360°)...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=2.0, + ellipse_semi_minor=1.0, + ellipse_orient=450.0, + ), +) +assert ellipse["semi_major_orientation"] == 900 + +print("position_confidence_ellipse: orientation correction (major < minor)...") +ellipse = position_confidence_ellipse( + GNSSReport( + ellipse_semi_major=1.0, + ellipse_semi_minor=2.0, + ellipse_orient=0.0, + ), +) +assert ellipse["semi_major_orientation"] == 900 + +#----------------------------------------------------------------------------------------- +print("gpsd connection to 127.0.0.1:2948...") gnss = GNSS( host="127.0.0.1", - port=2947, + port=2948, ) gnss.start() -for i in range(5): - time.sleep(1) +for i in range(50): + time.sleep(0.1) if gnss() is not None: break else: - raise RuntimeError("Can't connect to gpsd on 127.0.0.1:2947, is it running?") + raise RuntimeError("Can't connect to gpsd on 127.0.0.1:2948, is it running?") diff --git a/python/iot3/tests/test-iot3-mobility-message b/python/iot3/tests/test-iot3-mobility-message index 99ba20733..6e647f16d 100755 --- a/python/iot3/tests/test-iot3-mobility-message +++ b/python/iot3/tests/test-iot3-mobility-message @@ -15,10 +15,18 @@ msg = CAM( uuid="test_1234", gnss_report=GNSSReport(), ) +assert msg["message_type"] == "cam" +assert msg["version"] == "2.4.0" assert msg["message"]["basic_container"]["reference_position"]["latitude"] == 900000001 assert ( msg["message"]["basic_container"]["reference_position"]["longitude"] == 1800000001 ) +assert msg["message"]["basic_container"]["reference_position"]["altitude"]["value"] == 800001 +assert msg["message"]["basic_container"]["reference_position"]["altitude"]["confidence"] == 15 +assert msg["message"]["basic_container"]["reference_position"]["position_confidence_ellipse"]["semi_major"] == 4095 +assert msg["message"]["basic_container"]["reference_position"]["position_confidence_ellipse"]["semi_minor"] == 4095 +assert msg["message"]["basic_container"]["reference_position"]["position_confidence_ellipse"]["semi_major_orientation"] == 3601 +assert "basic_vehicle_container_high_frequency" in msg._message["message"]["high_frequency_container"] print("CAM with latitude and longitude...") msg = CAM( @@ -34,6 +42,108 @@ assert msg["message"]["basic_container"]["reference_position"]["longitude"] == 1 # Check with properties (SI values, degrees); ~6mm at Equator on surface of Earth assert math.fabs(msg.latitude - 43.6352120) < 5 * 10 ** -8 assert math.fabs(msg.longitude - 1.3745620) < 5 * 10 ** -8 +assert msg["message"]["basic_container"]["reference_position"]["altitude"]["value"] == 800001 +assert msg["message"]["basic_container"]["reference_position"]["altitude"]["confidence"] == 15 + +print("CAM with all GNSS fields...") +msg = CAM( + uuid="test_1234", + gnss_report=GNSSReport( + latitude=43.635212, + longitude=1.374562, + altitude=123.45, + speed=15.0, + track=45.0, + acceleration=1.2, + horizontal_error=0.5, + altitude_error=0.3, + ), +) +assert math.fabs(msg.altitude - 123.45) < 0.005 +hf = msg["message"]["high_frequency_container"]["basic_vehicle_container_high_frequency"] +assert hf["heading"]["value"] == 450 +assert hf["speed"]["value"] == 1500 +assert hf["longitudinal_acceleration"]["value"] == 12 +ellipse = msg["message"]["basic_container"]["reference_position"]["position_confidence_ellipse"] +assert ellipse["semi_major"] == 50 +assert ellipse["semi_minor"] == 50 +assert ellipse["semi_major_orientation"] == 0 + +print("CAM with RTK data...") +rtk_gnss_report = GNSSReport( + latitude=43.635212, + longitude=1.374562, + altitude=123.45, + speed=15.0, + track=45.0, + acceleration=1.2, + ellipse_semi_major=0.01, + ellipse_semi_minor=0.008, + ellipse_orient=30.0, + altitude_error=0.02, +) +msg = CAM( + uuid="test_1234", + gnss_report=rtk_gnss_report, +) +ellipse = msg["message"]["basic_container"]["reference_position"]["position_confidence_ellipse"] +assert ellipse["semi_major"] == 1 +assert ellipse["semi_minor"] == 1 +assert ellipse["semi_major_orientation"] == 300 +assert msg["message"]["basic_container"]["reference_position"]["altitude"]["confidence"] == 1 + +print("CAM position_confidence_ellipse from GST data...") +msg = CAM( + uuid="test_1234", + gnss_report=GNSSReport( + latitude=43.635212, + longitude=1.374562, + ellipse_semi_major=0.8, + ellipse_semi_minor=0.5, + ellipse_orient=30.0, + ), +) +ellipse = msg["message"]["basic_container"]["reference_position"]["position_confidence_ellipse"] +assert ellipse["semi_major"] == 80 +assert ellipse["semi_minor"] == 50 +assert ellipse["semi_major_orientation"] == 300 + +print("CAM position_confidence_ellipse from GST data out of range...") +msg = CAM( + uuid="test_1234", + gnss_report=GNSSReport( + latitude=43.635212, + longitude=1.374562, + ellipse_semi_major=42, + ellipse_semi_minor=42, + ellipse_orient=30.0, + ), +) +ellipse = msg["message"]["basic_container"]["reference_position"]["position_confidence_ellipse"] +assert ellipse["semi_major"] == 4094 +assert ellipse["semi_minor"] == 4094 +assert ellipse["semi_major_orientation"] == 300 + +print("CAM altitude confidence from altitude_error...") +msg = CAM( + uuid="test_1234", + gnss_report=GNSSReport( + latitude=43.635212, + longitude=1.374562, + altitude_error=0.3, + ), +) +assert msg["message"]["basic_container"]["reference_position"]["altitude"]["confidence"] == 5 + +print("CAM altitude confidence unavailable...") +msg = CAM( + uuid="test_1234", + gnss_report=GNSSReport( + latitude=43.635212, + longitude=1.374562, + ), +) +assert msg["message"]["basic_container"]["reference_position"]["altitude"]["confidence"] == 15 print("CAM serialisation...") json_msg = msg.to_json() @@ -44,6 +154,7 @@ assert msg.msg_type == "cam" assert math.fabs(msg.latitude - 43.6352120) < 5 * 10 ** -8 assert math.fabs(msg.longitude - 1.3745620) < 5 * 10 ** -8 +#----------------------------------------------------------------------------------------- print("CPM with no P.O...") msg = CPM( uuid="test_1234", @@ -55,7 +166,6 @@ msg = CPM( assert "station_data_container" not in msg["message"] assert len(msg["message"]["perceived_object_container"]) == 0 -#----------------------------------------------------------------------------------------- print("CPM of a car with some P.O...") msg = CPM( uuid="test_1234", @@ -223,7 +333,7 @@ msg = DENM( cause=DENM.Cause.trafficCondition, ) # UNIX time is a float that encodes seconds, while ETSI time is an int that -# encodes miliseconds, so we should not have a delta of 1ms or more +# encodes milliseconds, so we should not have a delta of 1ms or more assert math.fabs(detect_time - msg.detection_time) < 10 ** -3 seq_num = msg.sequence_number diff --git a/python/its-interqueuemanager/pyproject.toml b/python/its-interqueuemanager/pyproject.toml index a411634bc..aad4e8011 100644 --- a/python/its-interqueuemanager/pyproject.toml +++ b/python/its-interqueuemanager/pyproject.toml @@ -18,7 +18,7 @@ classifiers = [ ] license = "MIT" dependencies = [ - "iot3 @ git+https://github.com/Orange-OpenSource/its-client@594663bd225e552e55a201b650188740c0fae235#subdirectory=python/iot3", + "iot3 @ git+https://github.com/Orange-OpenSource/its-client@fe1cf253609d84a9abf857f31e6d4ae677e9d706#subdirectory=python/iot3", "requests==2.33.0", ] diff --git a/python/its-status/pyproject.toml b/python/its-status/pyproject.toml index 01edc3002..e8d7f01d1 100644 --- a/python/its-status/pyproject.toml +++ b/python/its-status/pyproject.toml @@ -18,7 +18,7 @@ classifiers = [ ] license = "MIT" dependencies = [ - "iot3 @ git+https://github.com/Orange-OpenSource/its-client@594663bd225e552e55a201b650188740c0fae235#subdirectory=python/iot3", + "iot3 @ git+https://github.com/Orange-OpenSource/its-client@fe1cf253609d84a9abf857f31e6d4ae677e9d706#subdirectory=python/iot3", "linuxfd==1.5", "psutil==5.8.0", ] diff --git a/python/its-vehicle/its-vehicle.cfg b/python/its-vehicle/its-vehicle.cfg index 850b6cb79..ccccedb94 100644 --- a/python/its-vehicle/its-vehicle.cfg +++ b/python/its-vehicle/its-vehicle.cfg @@ -104,6 +104,3 @@ host = HOSTNAME # How long to keep the last measurement when no new one comes (e.g. when # in a tunnel...); default: 2.0 # persistence = SEC -# Type of heuristic to gather messages, either "order" or "timestamp"; -# "order" is almost always the most sensible (see code), default: "order" -# heuristic = TYPE diff --git a/python/its-vehicle/pyproject.toml b/python/its-vehicle/pyproject.toml index 515618d25..839fecc05 100644 --- a/python/its-vehicle/pyproject.toml +++ b/python/its-vehicle/pyproject.toml @@ -18,7 +18,7 @@ classifiers = [ ] license = "MIT" dependencies = [ - "iot3 @ git+https://github.com/Orange-OpenSource/its-client@594663bd225e552e55a201b650188740c0fae235#subdirectory=python/iot3", + "iot3 @ git+https://github.com/Orange-OpenSource/its-client@fe1cf253609d84a9abf857f31e6d4ae677e9d706#subdirectory=python/iot3", "its-quadkeys @ git+https://github.com/Orange-OpenSource/its-client@fb9261360ee76dbabff8b3e6465c2941aec0ee70#subdirectory=python/its-quadkeys", "linuxfd==1.5", ] diff --git a/python/its-vehicle/src/its_vehicle/client.py b/python/its-vehicle/src/its_vehicle/client.py index 34ee01572..e1051f36f 100644 --- a/python/its-vehicle/src/its_vehicle/client.py +++ b/python/its-vehicle/src/its_vehicle/client.py @@ -8,10 +8,10 @@ import linuxfd import logging import threading -from .gpsd import GNSSProvider from iot3.core.mqtt import MqttClient +from iot3.mobility.cam import CAM +from iot3.mobility.gnss import GNSS from .roi import RegionOfInterest -from .its.cam import CooperativeAwarenessMessage as CAM class ITSClient: @@ -23,7 +23,7 @@ def __init__( self, *, cfg: dict, - gpsd: GNSSProvider, + gpsd: GNSS, mqtt_main: MqttClient, mqtt_mirror: MqttClient = None, ): @@ -55,12 +55,8 @@ def __init__( f"configuration key general.topic-sub-prefix must end in a / ({self.cfg['topic-sub-prefix']})" ) - self.pub_topic_root = ( - self.cfg["topic-pub-prefix"] - + ITSClient.TYPES[self.cfg["type"]]["topic"] - + "/" - + self.cfg["instance-id"] - + "/" + self.pub_topic_template = ( + f"{self.cfg['topic-pub-prefix']}{{msg_type}}/{{source_uuid}}/{{quadkey}}" ) self.roi = RegionOfInterest( @@ -111,7 +107,7 @@ def _loop(self): if self.should_stop: break - gnss_report = self.gpsd.get() + gnss_report = self.gpsd() if ( gnss_report is None or gnss_report.latitude is None @@ -119,13 +115,6 @@ def _loop(self): ): continue - quadkey = its_quadkeys.QuadKey( - ( - gnss_report.latitude, - gnss_report.longitude, - self.cfg["depth"], - ) - ) # Update RoI before we send a message, so that # we do not miss it... roi_topics = set() @@ -140,7 +129,8 @@ def _loop(self): + "/#" ), self.roi.get( - quadkey=quadkey, + latitude=gnss_report.latitude, + longitude=gnss_report.longitude, speed=gnss_report.speed, msg_type=msg_type, ), @@ -152,7 +142,10 @@ def _loop(self): uuid=self.cfg["instance-id"], gnss_report=gnss_report, ) - topic = self.pub_topic_root + quadkey.to_str("/") + topic = msg.topic( + template=self.pub_topic_template, + depth=self.cfg["depth"], + ) msg_json = msg.to_json() self.mqtt_main.publish(topic=topic, payload=msg_json) if self.mqtt_mirror and not self.cfg["mirror-self"]: @@ -176,7 +169,7 @@ def msg_cb( **_kwargs, ): logging.debug( - "received mesage on %s: %s", + "received message on %s: %s", topic[:16] + "..." if len(topic) > 16 else "", payload[:16].decode(errors="backslashreplace") + ("..." if len(payload) > 16 else ""), diff --git a/python/its-vehicle/src/its_vehicle/gpsd.py b/python/its-vehicle/src/its_vehicle/gpsd.py deleted file mode 100644 index b72312430..000000000 --- a/python/its-vehicle/src/its_vehicle/gpsd.py +++ /dev/null @@ -1,363 +0,0 @@ -# Software Name: its-vehicle -# SPDX-FileCopyrightText: Copyright (c) 2023 Orange -# SPDX-License-Identifier: MIT -# Author: Yann E. MORIN - -import dataclasses -import datetime -import errno -import json -import logging -import os -import select -import socket -import threading -import time - - -@dataclasses.dataclass(frozen=True) -class GNSSReport: - """A GNSS report. - - All values in SI units, with arbitrary precision. - Any value (but timestamp) may be None, when unknown or unavailable. - """ - - # UNIX timestamp this object was created at, with arbitrary sub-second precision - timestamp: float - # Time as sent by the GNSS service, with arbitrary sub-second precision - time: float | None = None - longitude: float | None = None - latitude: float | None = None - altitude: float | None = None - # Speed over ground (i.e. without vertical component) - speed: float | None = None - # Acceleration - acceleration: float | None = None - # Orientation from true North (geographic North, not magnetic North!) - track: float | None = None - # Semi-major/minor and orientation for error ellipse - major: float | None = None - minor: float | None = None - orient: float | None = None - # True and magnetic headings (true heading may or may not be equal to - # track, above) - true_heading: float | None = None - magnetic_heading: float | None = None - - -class GNSSProvider: - ClassesOfInterest = ["TPV", "GST", "ATT"] - DefaultData = { - "TPV": None, - "GST": None, - "ATT": None, - } - - def __init__(self, *, cfg): - logging.debug("creating a gpsd GNSS client") - - self.cfg = dict(cfg) - - # Even if we do have code for the "timestamp" heuristic, it is - # not officially supported, for being untested and having other - # drawbacks. Only explicitly accept the "order" heuristic. - if self.cfg["heuristic"] != "order": - raise NotImplementedError( - f"gpsd '{self.cfg['heuristic']}' heuristic not supported", - ) - - # Type coercion - self.cfg["port"] = int(self.cfg["port"]) - self.cfg["persistence"] = float(self.cfg["persistence"]) - logging.debug("starting gpsd provider with: %s", repr(self.cfg)) - - self.data = None - self.sock = None - self.sock_fd = None - self.should_stop = False - self.event_fd = os.eventfd(0) - self.poll = select.poll() - self.poll.register(self.event_fd, select.POLLIN) - self.thread = threading.Thread( - target=self._loop, - name="gnss.gpsd", - daemon=True, - ) - - def start(self): - logging.debug("starting gpsd GNSS client") - self.thread.start() - - def stop(self, wait=True): - logging.debug("stopping gpsd GNSS client") - self.should_stop = True - os.eventfd_write(self.event_fd, 1) - if wait: - self.join() - - def join(self): - self.thread.join() - logging.debug("stopped gpsd GNSS client") - - def get(self) -> GNSSReport | None: - if self.data: - now = datetime.datetime.now().timestamp() - if now - self.data.timestamp > self.cfg["persistence"]: - self.data = None - return self.data - - def _connect(self): - logging.debug("connecting to %s:%s", self.cfg["host"], self.cfg["port"]) - if self.sock is not None: - raise RuntimeError("Already connected") - - self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.sock.settimeout(2.0) - self.sock.connect((self.cfg["host"], self.cfg["port"])) - self.sock_fd = self.sock.makefile("rwb") - # sock.makefile() objects do not have a fileno() method, - # so we must watch the actual socket. - self.poll.register(self.sock, select.POLLIN) - logging.debug("starting WATCH mode") - self.sock_fd.write('?WATCH={"enable":true,"json":true};\n'.encode()) - self.sock_fd.flush() - self._data = self._new_epoch_data() - logging.debug("connected and listening") - - def _new_epoch_data(self): - data = dict(GNSSProvider.DefaultData) - for k in data: - data[k] = dict() - - return data - - def _disconnect(self): - logging.debug("disconnecting") - if self.sock_fd: - logging.debug("closing sock_fd") - self.sock_fd.close() - self.sock_fd = None - # The socket is only registered after we create sock_fd. - # so if we have a self.sock_fd, we had a self.sock. - self.poll.unregister(self.sock) - if self.sock: - logging.debug("closing sock") - self.sock.close() - self.sock = None - logging.debug("disconnected") - - def _loop(self): - logging.debug("gpsd GNSS client started") - while True: - if self.should_stop: - break - - if not self.sock: - try: - self._connect() - except ( - socket.timeout, - TimeoutError, - ConnectionRefusedError, - OSError, - ) as e: - logging.debug("connection failed: %s", repr(e)) - if type(e) is OSError: - # We're only interested in a few errno: - # - EBADF (9): Bad file descriptor (e.g. interrupted during connection) - # - EHOSTUNREACH (113): No route to host - # If not, just bubble the error up... - if e.errno not in [errno.EBADF, errno.EHOSTUNREACH]: - raise - # if there's no pending event, loop to retry the connection - if not self.poll.poll(0): - self._disconnect() - time.sleep(1) - continue - - poll_lst = self.poll.poll() - for fd, _ in poll_lst: - if fd == self.event_fd: - logging.debug("event, maybe we need to stop?") - break - elif fd == self.sock.fileno(): - logging.debug("gpsd is talking") - try: - self._read_sock() - except (socket.timeout, TimeoutError, ConnectionResetError) as e: - logging.debug("connection error: %s", repr(e)) - self._disconnect() - else: - raise RuntimeError("Unexpected filedescriptor while polling") - - # Out of the loop, cleanup and close - self._disconnect() - - def _read_sock(self): - # Socket-related exceptions are caught by the caller - msg_json = self.sock_fd.readline() - # When the socket got severed, we don't always notice (no idea - # why we don't always get TimeoutError or ConnectionResetError) - # but we get a short-read, which gives an empty message. When - # the socket is not in error, we never get a short read, so an - # empty message is a very good indication that the socket has - # some issue... - if not msg_json: - raise ConnectionResetError("short read") - - try: - msg = json.loads(msg_json) - except json.decoder.JSONDecodeError: - logging.debug( - "invalid JSON message: %s...%s of %d bytes", - msg[:16], - msg[-16:], - len(msg), - ) - # The GPSD protocol specifies a maximum length of messages, - # and that, as a consequence, the JSON sentence may get - # truncated. So, we just ignore any invalid JSON sentence. - return - - try: - if msg["class"] not in GNSSProvider.ClassesOfInterest: - logging.debug( - "not an interesting message (%s)", - msg["class"], - ) - return - except KeyError: - # Hopefully the first 42 bytes will be enough to recognise - # why the message had no class. - logging.debug("message with no 'class' [%s]", msg_json[:42]) - return - - # TPV, GST, ATT (and others) messages are emitted as separate - # json sentences, but they are usually correlated (ATT is - # explicitly documented to be "synchronous to the GNSS epoch". - # However, we don't know beforehand 1. in which order they will - # be emitted, and 2. if they will be emitted at all. There is a - # 'time' field documented for all those messages, but it may be - # missing, or its value may be way off (the documentation says: - # "May be absent if the mode is not 2D or 3D. May be present, - # but invalid, if there is no fix. Verify 3 consecutive 3D fixes - # before believing it is UTC. Even then it may be off by several - # seconds until the current leap seconds is known"). So, we - # can't rely on that field to aggregate correlated messages. - # - # Warning: last but not least, some messages can be emitted at - # least twice in a single epoch; this is the case for the SKY - # message, with the first message having all the satellites data - # but the second having none of it (not even nSat or uSat). - # So care must be had when storing such message. Fortunately, - # we've never seen ATT or GST messages being duplicated so far - # (for the single good reason that we have no GNSS device at - # hand that provide the corresponding data...). - if self.cfg["heuristic"] == "order": - # So, we use a crude heuristic: we assume that the TPV - # message is the last to be emitted in a GNSS epoch, so we - # store all messages we receive, and when we get a TPV one, - # we bundle everything we have about this epoch, queue it - # for further computations, and drop all the stored messages - # to start a new epoch afresh. - - # .update() so that messages accumulate rather than replace - # any previous one (critical for SKY messages for example, - # and even though we don't use those, we have no guarantee - # those we do listen for don't behave similarly). - self._data[msg["class"]].update(msg) - if msg["class"] == "TPV": - self._set_data() - self._data = self._new_epoch_data() - - elif self.cfg["heuristic"] == "timestamp": - # Altenate heuristic, based on the 'time' field: - # if: - # - the current message has a 'time' field, and - # - all the stored messages have a 'time' field, and - # - the 'time' field of the current message is greater - # than the 'time' field of stored messages - # then: - # - send all stored messages - # - drop stored messages - # - store the current message - # else: - # - store the current message - # if: - # - the current message is a 'TPV', and - # - any of the stored message has no 'time' field - # then: - # - send all stored messages - # - drop all stored messages - # - # The advantage of this time-based heuristic, is that we are - # (to a great extent) pretty sure that all the messages we did - # aggregate so far and that we are sending as a single unit, - # are strongly correlated one to the others, so this makes for - # a good unit to work KPIs and other computations on. - # - # The disadvantage, though, and this is a big one, is that we - # send data of a specific epoch at the begining of the next - # epoch. So, we get to do our KPIs and computations on data that - # is now aged of one period (e.g. 0.2s for a 5Hz rate). This is - # a pretty big drawback, so much so that pursuing this heuristic - # is not very interesting... - # - # This is all a bit convoluted, though, but we keep it below - # for reference (it never got tested at all). - - any_msg = self._data[list(self._data)[0]] - if ( - "time" in msg - and all([("time" in self._data[c]) for c in self._data]) - and ( - datetime.datetime.fromisoformat(msg["time"]) - > datetime.datetime.fromisoformat(any_msg["time"]) - ) - ): - self._set_data() - self._data = dict(GNSSProvider.DefaultData) - self._data[msg["class"]] = msg - else: - self._data[msg["class"]] = msg - if msg["class"] == "TPV" and any( - [("time" not in self._data[c]) for c in self._data] - ): - self._set_data() - self._data = dict(GNSSProvider.DefaultData) - - def _set_data(self): - # We are guaranteed to have a TPV message - tpv = self._data["TPV"] - tpv_time = tpv.get("time", None) - if tpv_time is not None: - if tpv_time[-1] == "Z": - tpv_time = tpv_time[:-1] - tpv_time = datetime.datetime.fromisoformat(tpv_time).timestamp() - - extras = dict() - - # Not all GNSS receivers will yield ATT messages - if self._data["ATT"] is not None: - extras["acceleration"] = self._data["ATT"].get("acc_len", None) - extras["true_heading"] = self._data["ATT"].get("heading", None) - extras["magnetic_heading"] = self._data["ATT"].get("mheading", None) - - # Not all GNSS receivers will yield GST messages - if self._data["GST"] is not None: - extras["major"] = self._data["GST"].get("major", None) - extras["minor"] = self._data["GST"].get("minor", None) - extras["orient"] = self._data["GST"].get("orient", None) - - self.data = GNSSReport( - timestamp=time.time(), - time=tpv_time, - longitude=tpv.get("lon", None), - latitude=tpv.get("lat", None), - altitude=tpv.get("altHAE", None), - speed=tpv.get("speed", None), - track=tpv.get("track", extras.get("true_heading", None)), - **extras, - ) - logging.debug("gpsd GNSS client data available: %s", self.data) diff --git a/python/its-vehicle/src/its_vehicle/its/__init__.py b/python/its-vehicle/src/its_vehicle/its/__init__.py deleted file mode 100644 index ccde5dbf0..000000000 --- a/python/its-vehicle/src/its_vehicle/its/__init__.py +++ /dev/null @@ -1,78 +0,0 @@ -# Software Name: its-vehicle -# SPDX-FileCopyrightText: Copyright (c) 2023 Orange -# SPDX-License-Identifier: MIT -# Author: Yann E. MORIN - -import datetime as _datetime - - -class ETSI: - # ETSI EPOCH, as a UNIX timestamp (int) - EPOCH = int( - _datetime.datetime.fromisoformat("2004-01-01T00:00:00.000+00:00").timestamp() - ) - - @staticmethod - def generation_delta_time(timestamp: float) -> int: - return ( - SI2ETSI.seconds( - timestamp - ETSI.EPOCH, - SI2ETSI.MILLI_SECOND, - 0, - ) - % 65536 - ) - - -class SI2ETSI: - # Length units - METER = 1.0 - DECI_METER = METER / 10 - CENTI_METER = METER / 100 - - # Time units - SECOND = 1.0 - MILLI_SECOND = SECOND / 1_000 - - # Speed units - METER_PER_SECOND = METER / SECOND - CENTI_METER_PER_SECOND = CENTI_METER / SECOND - - # Acceleration units - METER_PER_SECOND_SECOND = METER / (SECOND * SECOND) - DECI_METER_PER_SECOND_SECOND = DECI_METER / (SECOND * SECOND) - - # Degrees angle units - DEGREE = 1.0 - DECI_DEGREE = DEGREE / 10 - DECI_MICRO_DEGREE = DEGREE / 10_000_000 - - @staticmethod - def meters(meters: float | None, scale: float, undef: int) -> int: - return SI2ETSI._do_convert(meters, scale, undef) - - @staticmethod - def seconds(seconds: float | None, scale: float, undef: int) -> int: - return SI2ETSI._do_convert(seconds, scale, undef) - - @staticmethod - def meters_per_second(mps: float | None, scale: float, undef: int) -> int: - return SI2ETSI._do_convert(mps, scale, undef) - - @staticmethod - def meters_per_second_second(mpss: float | None, scale: float, undef: int) -> int: - return SI2ETSI._do_convert(mpss, scale, undef) - - @staticmethod - def degrees(degrees: float | None, scale: float, undef: int) -> int: - return SI2ETSI._do_convert(degrees, scale, undef) - - @staticmethod - def _do_convert(value: float | None, scale: float, undef: int) -> int: - return undef if value is None else int(round(value / scale)) - - -__all__ = [ - "ETSI", - "SI2ETSI", -] diff --git a/python/its-vehicle/src/its_vehicle/its/cam.py b/python/its-vehicle/src/its_vehicle/its/cam.py deleted file mode 100644 index 4e9d71965..000000000 --- a/python/its-vehicle/src/its_vehicle/its/cam.py +++ /dev/null @@ -1,107 +0,0 @@ -# Software Name: its-vehicle -# SPDX-FileCopyrightText: Copyright (c) 2023 Orange -# SPDX-License-Identifier: MIT -# Author: Yann E. MORIN - -import datetime -import hashlib -import json -from . import ETSI, SI2ETSI -from ..gpsd import GNSSReport - - -class CooperativeAwarenessMessage: - def __init__( - self, - *, - uuid: str, - gnss_report: GNSSReport, - ): - self.cam = dict( - { - "type": "cam", - "origin": "self", - "version": "1.1.3", - "source_uuid": uuid, - "timestamp": ( - SI2ETSI.seconds( - datetime.datetime.now(datetime.timezone.utc).timestamp(), - SI2ETSI.MILLI_SECOND, - 0, - ) - ), - "message": { - "protocol_version": 1, - "station_id": self.station_id(uuid), - "generation_delta_time": ( - ETSI.generation_delta_time(gnss_report.timestamp) - ), - "basic_container": { - "station_type": 5, - "reference_position": { - "latitude": SI2ETSI.degrees( - gnss_report.latitude, - SI2ETSI.DECI_MICRO_DEGREE, - 900000001, - ), - "longitude": SI2ETSI.degrees( - gnss_report.longitude, - SI2ETSI.DECI_MICRO_DEGREE, - 1800000001, - ), - "altitude": SI2ETSI.meters( - gnss_report.altitude, - SI2ETSI.CENTI_METER, - 800001, - ), - }, - "confidence": { - "position_confidence_ellipse": { - "semi_major_confidence": 10, - "semi_minor_confidence": 50, - "semi_major_orientation": 1, - }, - "altitude": 1, - }, - }, - "high_frequency_container": { - "heading": SI2ETSI.degrees( - gnss_report.track, - SI2ETSI.DECI_DEGREE, - 3601, - ), - "speed": SI2ETSI.meters_per_second( - gnss_report.speed, - SI2ETSI.CENTI_METER_PER_SECOND, - 16383, - ), - "longitudinal_acceleration": ( - SI2ETSI.meters_per_second_second( - gnss_report.acceleration, - SI2ETSI.DECI_METER_PER_SECOND_SECOND, - 161, - ) - ), - "drive_direction": 0, - "vehicle_length": 40, - "vehicle_width": 20, - "confidence": { - "heading": 2, - "speed": 3, - "vehicle_length": 0, - }, - }, - }, - }, - ) - - @staticmethod - def station_id(uuid: str) -> int: - return int( - hashlib.sha256(uuid.encode()).hexdigest()[:6], - 16, - ) - - def to_json(self) -> str: - # Return the densest possible JSON sentence - return json.dumps(self.cam, separators=(",", ":")) diff --git a/python/its-vehicle/src/its_vehicle/main.py b/python/its-vehicle/src/its_vehicle/main.py index a29c7080c..7cb7fe4c2 100644 --- a/python/its-vehicle/src/its_vehicle/main.py +++ b/python/its-vehicle/src/its_vehicle/main.py @@ -12,7 +12,7 @@ import signal import sys from . import client -from . import gpsd +from iot3.mobility.gnss import GNSS CFG = "/etc/its/vehicle.cfg" DEFAULTS = { @@ -40,7 +40,6 @@ "host": "127.0.0.1", "port": 2947, "persistence": 2.0, - "heuristic": "order", }, } @@ -124,7 +123,11 @@ def _set_default(section, key, default): ) otel_opts["span_ctxmgr_cb"] = otel.span - gnss = gpsd.GNSSProvider(cfg=cfg["gpsd"]) + gnss = GNSS( + host=cfg["gpsd"]["host"], + port=int(cfg["gpsd"]["port"]), + persistence=float(cfg["gpsd"]["persistence"]), + ) def _msg_cb(*args, **kwargs): its_client.msg_cb(*args, **kwargs) @@ -201,6 +204,7 @@ def term_handler(_signum: int, _frame): if mqtt_mirror is not None: mqtt_mirror.stop() gnss.stop() + gnss.join() if otel: otel.stop() diff --git a/python/its-vehicle/src/its_vehicle/roi.py b/python/its-vehicle/src/its_vehicle/roi.py index 36d26a46c..2a08e1acd 100644 --- a/python/its-vehicle/src/its_vehicle/roi.py +++ b/python/its-vehicle/src/its_vehicle/roi.py @@ -19,9 +19,11 @@ def __init__( def get( self, *, - quadkey: its_quadkeys.QuadKey, speed: float, msg_type: str, + quadkey: its_quadkeys.QuadKey | None = None, + latitude: float | None = None, + longitude: float | None = None, ): depth = self.depths[msg_type] if speed is not None: @@ -41,7 +43,14 @@ def get( # better approach would be compute the quadkeys enclosed in a # circle, but that's non-obvious... - shallow = quadkey.make_shallower(depth) + if quadkey is not None: + shallow = quadkey.make_shallower(depth) + elif latitude is not None and longitude is not None: + shallow = its_quadkeys.QuadKey((latitude, longitude, depth)) + else: + raise RuntimeError( + "No latitude and/or longitude, and no quadkey given, can't compute RoI" + ) roi = shallow.neighbours(as_zone=True) roi.add(shallow)