From 2f0c63a165c2b46e66af69b38b92415d083651a2 Mon Sep 17 00:00:00 2001 From: Luke Berndt Date: Sun, 3 Mar 2024 10:49:03 -0500 Subject: [PATCH] Support for occlusion mapping --- skyscan-c2.env | 1 + skyscan-c2/c2_pub_sub.py | 45 +++++++++++++++-- template/Dockerfile | 8 ---- template/poetry.lock | 15 ------ template/pyproject.toml | 16 ------- template/template_pub_sub.py | 93 ------------------------------------ 6 files changed, 41 insertions(+), 137 deletions(-) delete mode 100644 template/Dockerfile delete mode 100644 template/poetry.lock delete mode 100644 template/pyproject.toml delete mode 100644 template/template_pub_sub.py diff --git a/skyscan-c2.env b/skyscan-c2.env index 27654fb..4181f2d 100644 --- a/skyscan-c2.env +++ b/skyscan-c2.env @@ -10,4 +10,5 @@ TRIPOD_LATITUDE= TRIPOD_LONGITUDE= TRIPOD_ALITUDE= OBJECT_DISTANCE_THRESHOLD=1000000000.0 +MAPPING_FILEPATH=/data/mapping/occlusion.json LOG_LEVEL=INFO diff --git a/skyscan-c2/c2_pub_sub.py b/skyscan-c2/c2_pub_sub.py index 321a181..b57a6aa 100644 --- a/skyscan-c2/c2_pub_sub.py +++ b/skyscan-c2/c2_pub_sub.py @@ -44,6 +44,7 @@ def __init__( min_tilt: float, min_altitude: float, max_altitude: float, + mapping_filepath: str, object_distance_threshold: str, device_latitude: str, device_longitude: str, @@ -79,6 +80,7 @@ def __init__( self.device_latitude = float(device_latitude) self.device_longitude = float(device_longitude) self.device_altitude = float(device_altitude) + self.mapping_filepath = mapping_filepath self.object_distance_threshold = float(object_distance_threshold) self.min_tilt = min_tilt self.min_altitude = min_altitude @@ -98,6 +100,13 @@ def __init__( self.log_level = log_level self.override_object = None + if self.mapping_filepath == "": + self.occlusion_mapping_enabled = False + else: + with open(mapping_filepath) as f: + self.occlusion_mapping = json.load(f) + self.occlusion_mapping_enabled = True + # Compute tripod position in the geocentric (XYZ) coordinate # system self.r_XYZ_t = axis_ptz_utilities.compute_r_XYZ( @@ -155,6 +164,7 @@ def __init__( min_tilt = {min_tilt} min_altitude = {min_altitude} max_altitude = {max_altitude} + mapping_filepath = {mapping_filepath} object_distance_threshold = {object_distance_threshold} device_latitude = {device_latitude} device_longitude = {device_longitude} @@ -373,6 +383,28 @@ def _config_callback( self.min_altitude = config.get("min_altitude", self.min_altitude) self.max_altitude = config.get("max_altitude", self.max_altitude) + def _elevation_check(self: Any, azimuth: float, elevation: float) -> bool: + """Check if the elevation is within the acceptable range + + Args: + elevation (float): The elevation to check + + Returns: + bool: True if the elevation is within the acceptable range + """ + if occlusion_mapping_enabled: + occlusion_mapping_enabled = True + for obj in self.occlusion_mapping: + if obj["azimuth"] > azimuth: + if obj["elevation"] > elevation: + return False + else: + return True + break + return True + else: + return self.min_tilt <= elevation <= 90 + def _target_selection_callback( self: Any, _client: mqtt.Client, _userdata: Dict[Any, Any], msg: Any ) -> None: @@ -409,9 +441,11 @@ def _target_selection_callback( axis=1, ) - object_ledger_df["min_tilt_fail"] = ( - object_ledger_df["camera_tilt"] < self.min_tilt + object_ledger_df["min_tilt_fail"] = object_ledger_df.apply( + lambda x: self._elevation_check(x["camera_pan"], x["camera_tilt"]), + axis=1 ) + object_ledger_df["min_altitude_fail"] = ( object_ledger_df["altitude"] < self.min_altitude ) @@ -572,13 +606,14 @@ def main(self: Any) -> None: object_topic=str(os.environ.get("OBJECT_TOPIC")), prioritized_ledger_topic=str(os.environ.get("PRIORITIZED_LEDGER_TOPIC")), manual_override_topic=str(os.environ.get("MANUAL_OVERRIDE_TOPIC")), - device_latitude=str(os.environ.get("TRIPOD_LATITUDE")), - device_longitude=str(os.environ.get("TRIPOD_LONGITUDE")), - device_altitude=str(os.environ.get("TRIPOD_ALTITUDE")), + device_latitude=float(os.environ.get("TRIPOD_LATITUDE")), + device_longitude=float(os.environ.get("TRIPOD_LONGITUDE")), + device_altitude=float(os.environ.get("TRIPOD_ALTITUDE")), lead_time=float(os.environ.get("LEAD_TIME", 1.0)), min_tilt=float(os.environ.get("MIN_TILT", 0.0)), min_altitude=float(os.environ.get("MIN_ALTITUDE", 0.0)), max_altitude=float(os.environ.get("MAX_ALTITUDE", 100000000.0)), + mapping_filepath=str(os.environ.get("MAPPING_FILEPATH","")), object_distance_threshold=str(os.environ.get("OBJECT_DISTANCE_THRESHOLD")), log_level=str(os.environ.get("LOG_LEVEL", "INFO")), ) diff --git a/template/Dockerfile b/template/Dockerfile deleted file mode 100644 index 57a7678..0000000 --- a/template/Dockerfile +++ /dev/null @@ -1,8 +0,0 @@ -FROM iqtlabs/edgetech-core:latest -COPY pyproject.toml . -COPY poetry.lock . -RUN pip3 install poetry==1.1.5 -RUN poetry config virtualenvs.create false -RUN poetry install --no-dev -ADD template_pub_sub.py . -ENTRYPOINT [ "python3", "template_pub_sub.py" ] \ No newline at end of file diff --git a/template/poetry.lock b/template/poetry.lock deleted file mode 100644 index d3b9957..0000000 --- a/template/poetry.lock +++ /dev/null @@ -1,15 +0,0 @@ -[[package]] -name = "schedule" -version = "1.2.1" -description = "Job scheduling for humans." -category = "main" -optional = false -python-versions = ">=3.7" - -[metadata] -lock-version = "1.1" -python-versions = "^3.11" -content-hash = "c8ab7bee2ffcde3ecaea58fd3581cfcd09defdb89f8d22943dc4a158fe1f187f" - -[metadata.files] -schedule = [] diff --git a/template/pyproject.toml b/template/pyproject.toml deleted file mode 100644 index 120f740..0000000 --- a/template/pyproject.toml +++ /dev/null @@ -1,16 +0,0 @@ -[tool.poetry] -name = "examples" -version = "0.1.0" -description = "Template child class MQTT client that builds on top of EdgeTech-Core" -authors = ["arichadda "] -license = "Apache 2.0" - -[tool.poetry.dependencies] -python = "^3.11" -schedule = "^1.1.0" - -[tool.poetry.dev-dependencies] - -[build-system] -requires = ["poetry-core>=1.0.0"] -build-backend = "poetry.core.masonry.api" diff --git a/template/template_pub_sub.py b/template/template_pub_sub.py deleted file mode 100644 index fb96a29..0000000 --- a/template/template_pub_sub.py +++ /dev/null @@ -1,93 +0,0 @@ -import os -import json -import schedule -from time import sleep -from typing import Any, Dict -from datetime import datetime -import paho.mqtt.client as mqtt - -from base_mqtt_pub_sub import BaseMQTTPubSub - - -# inherit functionality from BaseMQTTPubSub parent this way -class TemplatePubSub(BaseMQTTPubSub): - def __init__( - self: Any, - env_variable: Any, - example_topic: str, - debug: bool = False, - **kwargs: Any, - ): - # Pass environment variables as parameters (include **kwargs) in super().__init__() - super().__init__(**kwargs) - self.env_variable = env_variable - self.example_topic = example_topic - # include debug version - self.debug = debug - - # Connect client in constructor - self.connect_client() - sleep(1) - self.publish_registration("Template Module Registration") - - def _example_callback( - self: Any, _client: mqtt.Client, _userdata: Dict[Any, Any], msg: Any - ) -> None: - # Decode message: - # Always publishing a JSON string with {timestamp: ____, data: ____,} - # TODO: more on this to come w/ a JSON header after talking to Rob - payload = json.loads(str(msg.payload.decode("utf-8"))) - - # Do something when a message is received - pass - - def main(self: Any) -> None: - # main function wraps functionality and always includes a while True - # (make sure to include a sleep) - - # include schedule heartbeat in every main() - schedule.every(10).seconds.do( - self.publish_heartbeat, payload="Template Module Heartbeat" - ) - - # If subscribing to a topic: - self.add_subscribe_topic(self.example_topic, self._example_callback) - - example_data = { - "timestamp": str(int(datetime.utcnow().timestamp())), - "data": "Example data payload", - } - - # example publish data every 10 minutes - schedule.every(10).minutes.do( - self.publish_to_topic, - topic_name="/example/topic", - publish_payload=json.dumps(example_data), - ) - - # or just publish once - self.publish_to_topic(self.example_publish_topic, json.dumps(example_data)) - - while True: - try: - # run heartbeat and anything else scheduled if clock is up - schedule.run_pending() - # include a sleep so loop does not run at CPU time - sleep(0.001) - - except Exception as e: - if self.debug: - print(e) - - -if __name__ == "__main__": - # instantiate an instance of the class - # any variables in BaseMQTTPubSub can be overridden using **kwargs - # and environment variables should be in the docker compose (in a .env file) - template = TemplatePubSub( - env_variable=str(os.environ.get("ENV_VARIABLE")), - example_topic=str(os.environ.get("EXAMPLE_TOPIC")), - mqtt_ip=str(os.environ.get("MQTT_IP")), - ) - # call the main function - template.main()