diff --git a/.ruff.toml b/.ruff.toml new file mode 100644 index 00000000..b57b7b68 --- /dev/null +++ b/.ruff.toml @@ -0,0 +1,22 @@ +target-version = "py310" +exclude = ["*_pb2.py"] + +[lint] +select = [ + "E", # pycodestyle + "F", # Pyflakes + "UP", # pyupgrade + "B", # flake8-bugbear + "SIM", # flake8-simplify + "I", # isort +] +ignore = [ + "B905", # new 'strict' argument for zip in python3.10 + "E501", # ignore line length errors for now + "E701", # multiple statements on one line, will be fixed by format + "E722", # do not use bare except + "E741", # ambiguous variable name (seems to trigger on l, I, and such) + "F841", # unused local variable + "SIM105", # contextlib.suppress is 3x slower than try-except + "SIM115", # use context manager to open files (only works in some places) +] diff --git a/cnc/server/main.py b/cnc/server/main.py index f36acec9..58978910 100755 --- a/cnc/server/main.py +++ b/cnc/server/main.py @@ -5,9 +5,10 @@ # # SPDX-License-Identifier: GPL-2.0-only -from gabriel_server.network_engine import server_runner -import logging import argparse +import logging + +from gabriel_server.network_engine import server_runner DEFAULT_PORT = 9099 DEFAULT_NUM_TOKENS = 2 diff --git a/cnc/server/swarm_controller.py b/cnc/server/swarm_controller.py index 165119b8..43273b74 100755 --- a/cnc/server/swarm_controller.py +++ b/cnc/server/swarm_controller.py @@ -5,22 +5,22 @@ # # SPDX-License-Identifier: GPL-2.0-only +import argparse import json +import logging import os import subprocess import sys -import logging +from urllib.parse import urlparse from zipfile import ZipFile -from google.protobuf.message import DecodeError -from google.protobuf import text_format + +import redis import requests -from cnc_protocol import cnc_pb2 -import argparse import zmq import zmq.asyncio -import redis -from urllib.parse import urlparse - +from cnc_protocol import cnc_pb2 +from google.protobuf import text_format +from google.protobuf.message import DecodeError logger = logging.getLogger(__name__) logger.setLevel(logging.DEBUG) @@ -102,7 +102,7 @@ def compile_mission(dsl_file, kml_file, drone_list, alt, compiler_file): def send_to_drone(msg, base_url, drone_list, cmd_front_cmdr_sock, redis): try: - logger.info(f"Sending request to drone...") + logger.info("Sending request to drone...") # Send the command to each drone for drone_id in drone_list: # check if the cmd is a mission @@ -117,7 +117,7 @@ def send_to_drone(msg, base_url, drone_list, cmd_front_cmdr_sock, redis): # store the record in redis key = redis.xadd( - f"commands", + "commands", {"commander": msg.commander_id, "drone": drone_id, "value": text_format.MessageToString(msg),} ) logger.debug(f"Updated redis under stream commands at key {key}") @@ -136,7 +136,7 @@ def listen_cmdrs(cmdr_sock, cmd_front_cmdr_sock, redis, alt, compiler_file): logger.info(f'Request received:\n{text_format.MessageToString(msg)}') except DecodeError: cmdr_sock.send(b'Error decoding protobuf. Did you send a cnc_pb2?') - logger.info(f'Error decoding protobuf. Did you send a cnc_pb2?') + logger.info('Error decoding protobuf. Did you send a cnc_pb2?') continue # get the drone list @@ -146,7 +146,7 @@ def listen_cmdrs(cmdr_sock, cmd_front_cmdr_sock, redis, alt, compiler_file): logger.info(f"drone list: {drone_list}") except json.JSONDecodeError: cmdr_sock.send(b'Error decoding drone list. Did you send a JSON list?') - logger.info(f'Error decoding drone list. Did you send a JSON list?') + logger.info('Error decoding drone list. Did you send a JSON list?') continue # Check if the command contains a mission and compile it if true diff --git a/cnc/server/telemetry.py b/cnc/server/telemetry.py index ec7eb1a0..a536e507 100755 --- a/cnc/server/telemetry.py +++ b/cnc/server/telemetry.py @@ -5,10 +5,11 @@ # # SPDX-License-Identifier: GPL-2.0-only +import argparse +import logging + from gabriel_server.network_engine import engine_runner from telemetry_engine import TelemetryEngine -import logging -import argparse SOURCE = 'telemetry' diff --git a/cnc/server/telemetry_engine.py b/cnc/server/telemetry_engine.py index 5fa01f83..9882645a 100644 --- a/cnc/server/telemetry_engine.py +++ b/cnc/server/telemetry_engine.py @@ -5,17 +5,18 @@ # # SPDX-License-Identifier: GPL-2.0-only -import time import datetime import logging -from gabriel_server import cognitive_engine -from gabriel_protocol import gabriel_pb2 -from cnc_protocol import cnc_pb2 -import redis import os -from PIL import Image +import time + import cv2 import numpy as np +import redis +from cnc_protocol import cnc_pb2 +from gabriel_protocol import gabriel_pb2 +from gabriel_server import cognitive_engine +from PIL import Image logger = logging.getLogger(__name__) logger.setLevel(logging.DEBUG) @@ -34,7 +35,7 @@ def __init__(self, args): os.makedirs(self.storage_path+"/raw") except FileExistsError: logger.info("Images directory already exists.") - logger.info("Storing detection images at {}".format(self.storage_path)) + logger.info(f"Storing detection images at {self.storage_path}") self.current_path = None self.publish = args.publish @@ -56,7 +57,7 @@ def handle(self, input_frame): result = None if input_frame.payload_type == gabriel_pb2.PayloadType.TEXT: - if extras.drone_id is not "": + if extras.drone_id != "": if extras.registering: logger.info(f"Drone [{extras.drone_id}] connected.") if not os.path.exists(f"{self.storage_path}/raw/{extras.drone_id}"): @@ -69,7 +70,7 @@ def handle(self, input_frame): result = gabriel_pb2.ResultWrapper.Result() result.payload_type = gabriel_pb2.PayloadType.TEXT - result.payload = "Telemetry updated.".encode(encoding="utf-8") + result.payload = b"Telemetry updated." self.updateDroneStatus(extras) elif input_frame.payload_type == gabriel_pb2.PayloadType.IMAGE: diff --git a/cnc/server/test/streaming_test_socket.py b/cnc/server/test/streaming_test_socket.py index f5265b18..fa72994c 100755 --- a/cnc/server/test/streaming_test_socket.py +++ b/cnc/server/test/streaming_test_socket.py @@ -2,13 +2,13 @@ # # SPDX-License-Identifier: GPL-2.0-only +import argparse import socket +import time + import cv2 import numpy as np -import time import zmq -import argparse -import sys HOST='' PORT=8485 @@ -90,8 +90,7 @@ def _main(): now = time.time() if now - lastprint > 5: print( - "avg fps: {0:.2f}".format( - (frames_received - lastcount) / 5) + f"avg fps: {(frames_received - lastcount) / 5:.2f}" ) print() lastcount = frames_received diff --git a/cnc/streamlit/commander.py b/cnc/streamlit/commander.py index 68543c16..4e94b66d 100644 --- a/cnc/streamlit/commander.py +++ b/cnc/streamlit/commander.py @@ -2,22 +2,17 @@ # # SPDX-License-Identifier: GPL-2.0-only -import streamlit as st -import pandas as pd -from streamlit_autorefresh import st_autorefresh +import datetime +import os + import folium -from folium.plugins import Draw -from streamlit_folium import st_folium -from st_keypressed import st_keypressed -import streamlit_antd_components as sac import redis -import os -from cnc_protocol import cnc_pb2 +import streamlit as st import zmq -import time -import numpy as np -import cv2 -import datetime +from cnc_protocol import cnc_pb2 +from st_keypressed import st_keypressed +from streamlit_autorefresh import st_autorefresh +from streamlit_folium import st_folium st.set_page_config( page_title="Commander", @@ -193,7 +188,7 @@ def rth(): ) if st.session_state.manual_control: - st.subheader(f":blue[Manual Control Enabled]") + st.subheader(":blue[Manual Control Enabled]") #st.subheader(":red[Manual Speed Controls]", divider="gray") st.sidebar.slider( key="pitch_speed", @@ -228,9 +223,9 @@ def rth(): step=5, ) elif st.session_state.rth_sent: - st.subheader(f":orange[Return to Home Initiated]") + st.subheader(":orange[Return to Home Initiated]") elif st.session_state.script_file is not None: - st.subheader(f":violet[Autonomous Mode Enabled]") + st.subheader(":violet[Autonomous Mode Enabled]") with c2: @@ -347,7 +342,7 @@ def rth(): #st.write(f":keyboard: {st.session_state.key_pressed}") st.session_state.key_pressed = st_keypressed() - if st.session_state.manual_control and st.session_state.selected_drone != None: + if st.session_state.manual_control and st.session_state.selected_drone is not None: req = cnc_pb2.Extras() req.commander_id = os.uname()[1] req.cmd.for_drone_id = st.session_state.selected_drone diff --git a/cnc/streamlit/commander_helper.py b/cnc/streamlit/commander_helper.py index 4b77a4bf..9b527614 100644 --- a/cnc/streamlit/commander_helper.py +++ b/cnc/streamlit/commander_helper.py @@ -5,15 +5,14 @@ # # SPDX-License-Identifier: GPL-2.0-only -import numpy as np -from gabriel_protocol import gabriel_pb2 -from gabriel_client.websocket_client import ProducerWrapper, WebsocketClient +import argparse import logging +import os + import zmq from cnc_protocol import cnc_pb2 -import argparse -import os -import asyncio +from gabriel_client.websocket_client import ProducerWrapper, WebsocketClient +from gabriel_protocol import gabriel_pb2 logger = logging.getLogger(__name__) @@ -49,7 +48,7 @@ async def producer(): extras = cnc_pb2.Extras() extras.commander_id = self.commander_id - if command != None: + if command is not None: extras.cmd.for_drone_id = command["drone_id"] if "kill" in command: extras.cmd.halt = True diff --git a/cnc/streamlit/overview.py b/cnc/streamlit/overview.py index 3f8797de..52a3746c 100644 --- a/cnc/streamlit/overview.py +++ b/cnc/streamlit/overview.py @@ -2,17 +2,26 @@ # # SPDX-License-Identifier: GPL-2.0-only +import json import os import time -import json from zipfile import ZipFile -from cnc_protocol import cnc_pb2 + import folium import streamlit as st -from streamlit_folium import st_folium +from cnc_protocol import cnc_pb2 from folium.plugins import MiniMap -from util import stream_to_dataframe, connect_redis, connect_zmq, get_drones, menu, COLORS, authenticated from st_keypressed import st_keypressed +from streamlit_folium import st_folium +from util import ( + COLORS, + authenticated, + connect_redis, + connect_zmq, + get_drones, + menu, + stream_to_dataframe, +) if "map_server" not in st.session_state: st.session_state.map_server = "Google Hybrid" @@ -69,7 +78,7 @@ def change_center(): f"telemetry.{st.session_state.tracking_selection}", "+", "-", 1 ) ) - for index, row in df.iterrows(): + for _index, row in df.iterrows(): st.session_state.center = [row["latitude"], row["longitude"]] @@ -132,8 +141,7 @@ def update_imagery(): drone_list.append(hsv_header) tabs = st.tabs(drone_list) - i = 0 - for d in drone_list: + for i, d in enumerate(drone_list): with tabs[i]: if d == detected_header: st.image(f"http://{st.secrets.webserver}/detected/latest.jpg?a={time.time()}", use_container_width=True) @@ -143,7 +151,7 @@ def update_imagery(): st.image(f"http://{st.secrets.webserver}/detected/hsv.jpg?a={time.time()}", use_container_width=True) else: st.image(f"http://{st.secrets.webserver}/raw/{d}/latest.jpg?a={time.time()}", use_container_width=True) - i += 1 + @st.fragment(run_every="1s") def draw_map(): m = folium.Map( @@ -164,9 +172,8 @@ def draw_map(): last_update = (int(df.index[0].split("-")[0])/1000) if time.time() - last_update < st.session_state.inactivity_time * 60: # minutes -> seconds coords = [] - i = 0 drone_name = k.split(".")[-1] - for index, row in df.iterrows(): + for i, (_index, row) in enumerate(df.iterrows()): if i % 10 == 0: coords.append([row["latitude"], row["longitude"]]) if i == 0: @@ -203,8 +210,6 @@ def draw_map(): ) ) - i += 1 - ls = folium.PolyLine(locations=coords, color=COLORS[marker_color]) ls.add_to(tracks) marker_color += 1 diff --git a/cnc/streamlit/pages/control.py b/cnc/streamlit/pages/control.py index b38bde76..8651aedd 100644 --- a/cnc/streamlit/pages/control.py +++ b/cnc/streamlit/pages/control.py @@ -2,18 +2,27 @@ # # SPDX-License-Identifier: GPL-2.0-only -import streamlit as st import asyncio import json -from zipfile import ZipFile -from st_keypressed import st_keypressed import os -from cnc_protocol import cnc_pb2 import time +from zipfile import ZipFile + import folium -from streamlit_folium import st_folium +import streamlit as st +from cnc_protocol import cnc_pb2 from folium.plugins import MiniMap -from util import stream_to_dataframe, get_drones, connect_redis, connect_zmq, menu, connect_redis_publisher, COLORS, authenticated +from st_keypressed import st_keypressed +from streamlit_folium import st_folium +from util import ( + COLORS, + authenticated, + connect_redis, + connect_redis_publisher, + connect_zmq, + menu, + stream_to_dataframe, +) st.set_page_config( page_title="Commander", @@ -196,7 +205,7 @@ def draw_map(): df = stream_to_dataframe(st.session_state.redis .xrevrange(f"telemetry.{st.session_state.selected_drone}", "+", "-", 1)) last_update = (int(df.index[0].split("-")[0])/1000) i = 0 - for index, row in df.iterrows(): + for _index, row in df.iterrows(): text = folium.DivIcon( icon_size="null", #set the size to null so that it expands to the length of the string inside in the div icon_anchor=(-20, 30), @@ -302,9 +311,9 @@ def draw_map(): c6.number_input(key = "imagery_framerate", label="Imagery Framerate", min_value=1, max_value=30, step=1, value=2, format="%0d") elif st.session_state.rth_sent: - mode = f":orange[Return to Home Initiated]" + mode = ":orange[Return to Home Initiated]" elif st.session_state.script_file is not None: - mode = f":violet[Autonomous Mode Enabled]" + mode = ":violet[Autonomous Mode Enabled]" status_container, imagery_container = st.columns(spec=[2, 3], gap="large") diff --git a/cnc/streamlit/pages/plan.py b/cnc/streamlit/pages/plan.py index b191a0d8..10d6de1e 100644 --- a/cnc/streamlit/pages/plan.py +++ b/cnc/streamlit/pages/plan.py @@ -3,15 +3,14 @@ # SPDX-License-Identifier: GPL-2.0-only import streamlit as st -from streamlit_ace import st_ace -from util import menu, authenticated import streamlit.components.v1 as components -from streamlit_oauth import OAuth2Component from google.auth.transport.requests import Request from google.oauth2.credentials import Credentials - from googleapiclient.discovery import build from googleapiclient.errors import HttpError +from streamlit_ace import st_ace +from streamlit_oauth import OAuth2Component +from util import authenticated, menu sample="""Task { Detect patrol_route { @@ -69,9 +68,8 @@ def fetch_mymaps(): client_id=st.secrets.oauth.client_id, client_secret=st.secrets.oauth.client_secret) - if not creds or not creds.valid: - if creds and creds.expired and creds.refresh_token: - creds.refresh(Request()) + if creds and not creds.valid and creds.expired and creds.refresh_token: + creds.refresh(Request()) try: service = build("drive", "v3", credentials=creds) diff --git a/cnc/streamlit/util.py b/cnc/streamlit/util.py index 51f6a229..0c70ea6b 100644 --- a/cnc/streamlit/util.py +++ b/cnc/streamlit/util.py @@ -2,13 +2,14 @@ # # SPDX-License-Identifier: GPL-2.0-only -import streamlit as st -import redis -import pandas as pd +import hmac import json -import zmq import time -import hmac + +import pandas as pd +import redis +import streamlit as st +import zmq DATA_TYPES = { "latitude": "float", diff --git a/droneDSL/python/interface/Task.py b/droneDSL/python/interface/Task.py index 8ac96b43..fc233c97 100644 --- a/droneDSL/python/interface/Task.py +++ b/droneDSL/python/interface/Task.py @@ -2,10 +2,11 @@ # # SPDX-License-Identifier: GPL-2.0-only -from abc import ABC, abstractmethod import functools import logging import threading +from abc import ABC, abstractmethod + from aenum import Enum logger = logging.getLogger(__name__) @@ -17,7 +18,7 @@ class TaskType(Enum): Avoid = 3 Test = 4 -class TaskArguments(): +class TaskArguments: def __init__(self, task_type, transitions_attributes, task_attributes): self.task_type = task_type self.task_attributes = task_attributes @@ -45,17 +46,17 @@ def get_task_id(self): def _exit(self): # kill all the transitions - logger.info(f"**************exit the task**************\n") + logger.info("**************exit the task**************\n") self.stop_trans() self.trigger_event_queue.put((self.task_id, "done")) def stop_trans(self): - logger.info(f"**************stopping the transitions**************\n") + logger.info("**************stopping the transitions**************\n") for trans in self.trans_active: if trans.is_alive(): trans.stop() trans.join() - logger.info(f"**************the transitions stopped**************\n") + logger.info("**************the transitions stopped**************\n") @classmethod @@ -73,8 +74,10 @@ async def wrapper(self, *args, **kwargs): return wrapper + @abstractmethod def pause(self): pass + @abstractmethod def resume(self): pass diff --git a/droneDSL/python/interface/Transition.py b/droneDSL/python/interface/Transition.py index a7978905..26fddd3b 100644 --- a/droneDSL/python/interface/Transition.py +++ b/droneDSL/python/interface/Transition.py @@ -1,7 +1,6 @@ -from abc import ABC, abstractmethod import logging import threading - +from abc import ABC, abstractmethod logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/droneDSL/python/project/task_defs/AvoidTask.py b/droneDSL/python/project/task_defs/AvoidTask.py index 1be64428..d64407b8 100644 --- a/droneDSL/python/project/task_defs/AvoidTask.py +++ b/droneDSL/python/project/task_defs/AvoidTask.py @@ -3,15 +3,17 @@ # SPDX-License-Identifier: GPL-2.0-only #from interfaces.Task import Task -import json -from json import JSONDecodeError -import time import asyncio +import json import logging +import time +from json import JSONDecodeError + from gabriel_protocol import gabriel_pb2 -from ..transition_defs.TimerTransition import TimerTransition from interface.Task import Task +from ..transition_defs.TimerTransition import TimerTransition + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -130,10 +132,10 @@ async def run(self): error = await self.computeError() logger.info(f"[ObstacleTask] Error {error}") await self.moveForwardAndAvoid(error) - except JSONDecodeError as e: - logger.error(f"[ObstacleTask]: Error decoding JSON") + except JSONDecodeError: + logger.error("[ObstacleTask]: Error decoding JSON") except Exception as e: - logger.error(f"[ObstacleTask] Threw an exception") + logger.error("[ObstacleTask] Threw an exception") logger.error(e) await asyncio.sleep(0.1) except Exception as e: diff --git a/droneDSL/python/project/task_defs/DetectTask.py b/droneDSL/python/project/task_defs/DetectTask.py index d14569ff..d7e78678 100644 --- a/droneDSL/python/project/task_defs/DetectTask.py +++ b/droneDSL/python/project/task_defs/DetectTask.py @@ -1,13 +1,13 @@ -from ..transition_defs.ObjectDetectionTransition import ObjectDetectionTransition -from ..transition_defs.TimerTransition import TimerTransition -from ..transition_defs.HSVDetectionTransition import HSVDetectionTransition -from interface.Task import Task -import asyncio import ast +import asyncio import logging -from gabriel_protocol import gabriel_pb2 +from interface.Task import Task + +from ..transition_defs.HSVDetectionTransition import HSVDetectionTransition +from ..transition_defs.ObjectDetectionTransition import ObjectDetectionTransition +from ..transition_defs.TimerTransition import TimerTransition logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/droneDSL/python/project/task_defs/SetHome.py b/droneDSL/python/project/task_defs/SetHome.py index 6117ab36..b4266dfe 100644 --- a/droneDSL/python/project/task_defs/SetHome.py +++ b/droneDSL/python/project/task_defs/SetHome.py @@ -2,9 +2,11 @@ # # SPDX-License-Identifier: GPL-2.0-only -from interface.Task import Task import ast +from interface.Task import Task + + class SetHome(Task): def __init__(self, drone, cloudlet, **kwargs): diff --git a/droneDSL/python/project/task_defs/TestTask.py b/droneDSL/python/project/task_defs/TestTask.py index 422eedea..1327fedf 100644 --- a/droneDSL/python/project/task_defs/TestTask.py +++ b/droneDSL/python/project/task_defs/TestTask.py @@ -1,10 +1,11 @@ -from ..transition_defs.TimerTransition import TimerTransition -from interface.Task import Task -import asyncio import ast +import asyncio import logging +from interface.Task import Task + +from ..transition_defs.TimerTransition import TimerTransition logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/droneDSL/python/project/task_defs/TrackTask.py b/droneDSL/python/project/task_defs/TrackTask.py index 076bc9ea..cc33b724 100644 --- a/droneDSL/python/project/task_defs/TrackTask.py +++ b/droneDSL/python/project/task_defs/TrackTask.py @@ -1,16 +1,18 @@ import asyncio -from json import JSONDecodeError -import sys import json -import numpy as np +import logging import math -from ..transition_defs.TimerTransition import TimerTransition -from interface.Task import Task +import sys import time -import logging +from json import JSONDecodeError + +import numpy as np from gabriel_protocol import gabriel_pb2 +from interface.Task import Task from scipy.spatial.transform import Rotation as R +from ..transition_defs.TimerTransition import TimerTransition + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -190,8 +192,7 @@ async def run(self): if last_seen is not None and int(time.time() - last_seen) > self.target_lost_duration: #if we have not found the target in N seconds trigger the done transition break - if result != None: - if result.payload_type == gabriel_pb2.TEXT: + if result is not None and result.payload_type == gabriel_pb2.TEXT: try: json_string = result.payload.decode('utf-8') json_data = json.loads(json_string) @@ -209,8 +210,8 @@ async def run(self): logger.info(f"[TrackTask]: Detected instance of {target}, tracking...") vels = await self.pid(box) await self.actuate(vels) - except JSONDecodeError as e: - logger.error(f"[TrackTask]: Error decoding json, ignoring") + except JSONDecodeError: + logger.error("[TrackTask]: Error decoding json, ignoring") except Exception as e: exc_type, exc_obj, exc_tb = sys.exc_info() logger.error(f"[TrackTask]: Exception encountered, {e}, line no {exc_tb.tb_lineno}") diff --git a/droneDSL/python/project/transition_defs/HSVDetectionTransition.py b/droneDSL/python/project/transition_defs/HSVDetectionTransition.py index 816e57e8..eb0b67cb 100644 --- a/droneDSL/python/project/transition_defs/HSVDetectionTransition.py +++ b/droneDSL/python/project/transition_defs/HSVDetectionTransition.py @@ -1,9 +1,9 @@ -from json import JSONDecodeError import json import logging -from venv import logger -from interface.Transition import Transition +from json import JSONDecodeError + from gabriel_protocol import gabriel_pb2 +from interface.Transition import Transition logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -23,8 +23,7 @@ def run(self): self.cloudlet.clearResults("openscout-object") while not self.stop_signal: result = self.cloudlet.getResults("openscout-object") - if (result != None): - if result.payload_type == gabriel_pb2.TEXT: + if result is not None and result.payload_type == gabriel_pb2.TEXT: try: json_string = result.payload.decode('utf-8') json_data = json.loads(json_string) @@ -35,7 +34,7 @@ def run(self): logger.info(f"**************Transition: Task {self.task_id}: detect condition met! {class_attribute}**************\n") self._trigger_event("hsv_detection") break - except JSONDecodeError as e: + except JSONDecodeError: logger.error(f'Error decoding json: {json_string}') except Exception as e: logger.info(e) diff --git a/droneDSL/python/project/transition_defs/ObjectDetectionTransition.py b/droneDSL/python/project/transition_defs/ObjectDetectionTransition.py index c2aa9bd3..8c4bb12f 100644 --- a/droneDSL/python/project/transition_defs/ObjectDetectionTransition.py +++ b/droneDSL/python/project/transition_defs/ObjectDetectionTransition.py @@ -1,10 +1,10 @@ -from json import JSONDecodeError import json import logging import time -from venv import logger -from interface.Transition import Transition +from json import JSONDecodeError + from gabriel_protocol import gabriel_pb2 +from interface.Transition import Transition logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -26,7 +26,7 @@ def run(self): while not self.stop_signal: # get result result = self.cloudlet.getResults("openscout-object") - if (result != None): + if result is not None: logger.info(f"**************Transition: Task {self.task_id}: detected payload! {result}**************\n") # Check if the payload type is TEXT, since your JSON seems to be text data if result.payload_type == gabriel_pb2.TEXT: @@ -45,11 +45,11 @@ def run(self): logger.info(f"**************Transition: Task {self.task_id}: detect condition met! {class_attribute}**************\n") self._trigger_event("object_detection") break - except JSONDecodeError as e: + except JSONDecodeError: logger.error(f'Error decoding json: {json_string}') except Exception as e: logger.info(e) # print("object stopping...\n") self._unregister() - \ No newline at end of file + diff --git a/droneDSL/python/project/transition_defs/TimerTransition.py b/droneDSL/python/project/transition_defs/TimerTransition.py index 11637693..d1a73f5e 100644 --- a/droneDSL/python/project/transition_defs/TimerTransition.py +++ b/droneDSL/python/project/transition_defs/TimerTransition.py @@ -1,5 +1,6 @@ import logging import threading + from interface.Transition import Transition logger = logging.getLogger(__name__) diff --git a/field-testing/avoidance/common.py b/field-testing/avoidance/common.py index 6849642c..385bab76 100755 --- a/field-testing/avoidance/common.py +++ b/field-testing/avoidance/common.py @@ -2,12 +2,13 @@ # # SPDX-License-Identifier: GPL-2.0-only -import numpy as np -import cv2 from operator import attrgetter +import cv2 +import numpy as np + -class KeyPointHistory(object): +class KeyPointHistory: def __init__(self): self.age = -1 self.lastFrameIdx = 0 @@ -44,7 +45,7 @@ def __str__(self): return str(dict((attr,getattr(self,attr)) for attr in dir(self) if not attr.startswith('_') and not callable(getattr(self,attr)))) -class Cluster(object): +class Cluster: def __init__(self,keypoints,img): self.mask = np.zeros_like(img) for kp in keypoints: @@ -81,31 +82,42 @@ def findCoM(mask): return x, y -trunc_coords = lambda shape,xy: [round(x) if x >= 0 and x <= dimsz else (0 if x < 0 else dimsz) +def trunc_coords(shape, xy): + return [round(x) if x >= 0 and x <= dimsz else (0 if x < 0 else dimsz) for dimsz,x in zip(shape[::-1],xy)] -bboverlap = lambda cl1,cl2: (cl1.p0[0] <= cl2.p1[0] and cl1.p1[0] >= cl2.p0[0]) and (cl1.p0[1] <= cl2.p1[1] and cl1.p1[1] >= cl2.p0[1]) +def bboverlap(cl1, cl2): + return (cl1.p0[0] <= cl2.p1[0] and cl1.p1[0] >= cl2.p0[0]) and (cl1.p0[1] <= cl2.p1[1] and cl1.p1[1] >= cl2.p0[1]) -overlap = lambda kp1,kp2,eps=0: (kp1.size//2+kp2.size//2+eps) > diffKP_L2(kp1,kp2) +def overlap(kp1, kp2, eps=0): + return (kp1.size//2+kp2.size//2+eps) > diffKP_L2(kp1,kp2) -diffKP_L2 = lambda kp0,kp1: np.sqrt((kp0.pt[0]-kp1.pt[0])**2 + (kp0.pt[1]-kp1.pt[1])**2) +def diffKP_L2(kp0, kp1): + return np.sqrt((kp0.pt[0]-kp1.pt[0])**2 + (kp0.pt[1]-kp1.pt[1])**2) -diffKP = lambda kp0,kp1: (kp0.pt[0]-kp1.pt[0], kp0.pt[1]-kp1.pt[1]) +def diffKP(kp0, kp1): + return (kp0.pt[0]-kp1.pt[0], kp0.pt[1]-kp1.pt[1]) -difftuple_L2 = lambda p0,p1: np.sqrt((p0[0]-p1[0])**2 + (p0[1]-p1[1])**2) +def difftuple_L2(p0, p1): + return np.sqrt((p0[0]-p1[0])**2 + (p0[1]-p1[1])**2) -difftuple = lambda p0,p1: (p1[0]-p0[0],p1[1]-p0[1]) +def difftuple(p0, p1): + return (p1[0]-p0[0],p1[1]-p0[1]) -inttuple = lambda *x: tuple(map(int,x)) +def inttuple(*x): + return tuple(map(int,x)) -roundtuple = lambda *x: tuple(map(int,map(round,x))) +def roundtuple(*x): + return tuple(map(int,map(round,x))) -avgKP = lambda keypoints: map(lambda x: sum(x)/len(keypoints),zip(*map(attrgetter('pt'),keypoints))) +def avgKP(keypoints): + return map(lambda x: sum(x)/len(keypoints),zip(*map(attrgetter('pt'),keypoints))) -toKeyPoint_cv = lambda kp: cv2.KeyPoint(kp.pt[0],kp.pt[1],kp.size,_angle=kp.angle,_response=kp.response,_octave=kp.octave,_class_id=kp.class_id) +def toKeyPoint_cv(kp): + return cv2.KeyPoint(kp.pt[0],kp.pt[1],kp.size,_angle=kp.angle,_response=kp.response,_octave=kp.octave,_class_id=kp.class_id) def reprObj(obj): - return "\n".join(["%s = %s" % (attr, getattr(obj, attr)) for attr in dir(obj) if not attr.startswith('_') and not callable(getattr(src,attr))]) + return "\n".join([f"{attr} = {getattr(obj, attr)}" for attr in dir(obj) if not attr.startswith('_') and not callable(getattr(obj,attr))]) def cvtIdx(pt,shape): return int(pt[1]*shape[1] + pt[0]) if hasattr(pt, '__len__') else map(int, (pt%shape[1], pt//shape[1])) diff --git a/field-testing/avoidance/midas_avoider.py b/field-testing/avoidance/midas_avoider.py index f3d72d88..8fc998ca 100644 --- a/field-testing/avoidance/midas_avoider.py +++ b/field-testing/avoidance/midas_avoider.py @@ -2,16 +2,13 @@ # # SPDX-License-Identifier: GPL-2.0-only -import olympe -from olympe.messages.ardrone3.Piloting import PCMD -from olympe.messages.ardrone3.PilotingState import GpsLocationChanged +import json import threading import time + import zmq -import json -import numpy as np -from datetime import datetime -import os +from olympe.messages.ardrone3.Piloting import PCMD +from olympe.messages.ardrone3.PilotingState import GpsLocationChanged FOLDER = "./avoidance/traces/" @@ -54,13 +51,10 @@ def run(self): try: vec = json.loads(self.sub_socket.recv_json(flags=zmq.NOBLOCK))[0]["vector"] print(f"Receiving detections: {vec}") - if self.hysteresis: - diff = vec - lastvec - else: - diff = 0 + diff = vec - lastvec if self.hysteresis else 0 lastvec = vec self.move_by_offsets(vec + diff) - except Exception as e: + except Exception: print(f"Actuating on last: {lastvec}") self.move_by_offsets(lastvec) time.sleep(0.05) diff --git a/field-testing/avoidance/sift_avoider.py b/field-testing/avoidance/sift_avoider.py index 9202b955..5065f662 100755 --- a/field-testing/avoidance/sift_avoider.py +++ b/field-testing/avoidance/sift_avoider.py @@ -4,26 +4,23 @@ # # SPDX-License-Identifier: GPL-2.0-only -import olympe -from olympe.messages.ardrone3.Piloting import PCMD -from olympe.messages.ardrone3.PilotingState import GpsLocationChanged +import sys +import time + import cv2 import numpy as np -import logging -from collections import OrderedDict -import time,sys -from matplotlib import pyplot as plt -import sys +from olympe.messages.ardrone3.Piloting import PCMD +from olympe.messages.ardrone3.PilotingState import GpsLocationChanged + sys.path.append('./avoidance/') -from common import * -import operator as op import math +import operator as op import threading -import zmq -import time -import traceback from datetime import datetime +import zmq +from common import Cluster, overlap + STREAM_FPS = 1 #used for ttc RATIO = 0.75 AGE = 10 @@ -119,7 +116,7 @@ def run(self): self.prev_image = self.image if vec is not None: self.move_by_offsets(vec) - except Exception as e: + except Exception: if lastvec: self.move_by_offsets(lastvec) time.sleep(0.05) @@ -140,10 +137,10 @@ def init_sift(self): self.id = 1 try: - self.roi = np.zeros(prev_img.shape,np.uint8) - scrapY, scrapX = prev_img.shape[0]//self.r, prev_img.shape[1]//(self.r + 1) + self.roi = np.zeros(self.prev_img.shape,np.uint8) + scrapY, scrapX = self.prev_img.shape[0]//self.r, self.prev_img.shape[1]//(self.r + 1) self.roi[scrapY:-scrapY, scrapX:-scrapX] = True - except Exception as e: + except Exception: pass def match(self): @@ -298,7 +295,7 @@ def get_offsets(self): cY = int(M["m01"] / M["m00"]) cv2.circle(dispim, (scrapX + cX, scrapY + cY), 5, (0, 255, 0), -1) cv2.putText(dispim, "safe", (scrapX + cX, scrapY + cY - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) - except Exception as e: + except Exception: pass self.prev_img = self.image diff --git a/field-testing/laptop-controller.py b/field-testing/laptop-controller.py index 75ce62cc..fbf1656d 100644 --- a/field-testing/laptop-controller.py +++ b/field-testing/laptop-controller.py @@ -2,34 +2,34 @@ # # SPDX-License-Identifier: GPL-2.0-only -import olympe -from olympe.messages.ardrone3.Piloting import TakeOff, Landing, PCMD, moveBy -from olympe.messages.ardrone3.PilotingState import AttitudeChanged, GpsLocationChanged, AltitudeChanged -from olympe.messages.skyctrl.CoPiloting import setPilotingSource -from olympe.messages.obstacle_avoidance import set_mode -from olympe.enums.obstacle_avoidance import mode -from olympe.messages.gimbal import set_target, attitude -from olympe.enums.gimbal import control_mode -from olympe.video.renderer import PdrawRenderer -import threading -import time -import queue +import argparse import logging +import queue import subprocess -import cv2 -from pynput.keyboard import Listener, Key, KeyCode +import threading +import time from collections import defaultdict +from datetime import datetime from enum import Enum -from time import sleep -import numpy as np -import math -import os + +import cv2 +import olympe import zmq -import json -from trackers import dynamic, static, parrot -from avoidance import sift_avoider, midas_avoider -import argparse -from datetime import datetime +from avoidance import midas_avoider, sift_avoider +from olympe.enums.gimbal import control_mode +from olympe.enums.obstacle_avoidance import mode +from olympe.messages.ardrone3.Piloting import PCMD, Landing, TakeOff +from olympe.messages.ardrone3.PilotingState import ( + AltitudeChanged, + AttitudeChanged, + GpsLocationChanged, +) +from olympe.messages.gimbal import attitude, set_target +from olympe.messages.obstacle_avoidance import set_mode +from olympe.messages.skyctrl.CoPiloting import setPilotingSource +from olympe.video.renderer import PdrawRenderer +from pynput.keyboard import Key, KeyCode, Listener +from trackers import dynamic DRONE_IP = "192.168.42.1" # Real drone no controller @@ -96,10 +96,7 @@ def _on_press(self, key): self._key_pressed[key.char] = True elif isinstance(key, Key): self._key_pressed[key] = True - if self._key_pressed[self._ctrl_keys[Ctrl.QUIT]]: - return False - else: - return True + return not self._key_pressed[self._ctrl_keys[Ctrl.QUIT]] def _on_release(self, key): if isinstance(key, KeyCode): @@ -341,7 +338,7 @@ def run(self): continue try: self.send_yuv_frame_to_server(yuv_frame) - except Exception as e: + except Exception: #print(e) pass finally: diff --git a/field-testing/olympe-test.py b/field-testing/olympe-test.py index 99bb6e8f..497e312f 100644 --- a/field-testing/olympe-test.py +++ b/field-testing/olympe-test.py @@ -2,8 +2,9 @@ # # SPDX-License-Identifier: GPL-2.0-only -import olympe import time + +import olympe from olympe.messages.gimbal import set_target if __name__ == "__main__": diff --git a/field-testing/streaming.py b/field-testing/streaming.py index f8a53049..091ef4a5 100644 --- a/field-testing/streaming.py +++ b/field-testing/streaming.py @@ -18,15 +18,13 @@ import time import olympe -from olympe.messages.ardrone3.Piloting import TakeOff, Landing -from olympe.messages.ardrone3.Piloting import moveBy -from olympe.messages.ardrone3.PilotingState import FlyingStateChanged +from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged +from olympe.messages.ardrone3.Piloting import Landing, TakeOff, moveBy from olympe.messages.ardrone3.PilotingSettings import MaxTilt from olympe.messages.ardrone3.PilotingSettingsState import MaxTiltChanged -from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged +from olympe.messages.ardrone3.PilotingState import FlyingStateChanged from olympe.video.renderer import PdrawRenderer - olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}}) DRONE_IP = "192.168.42.1" diff --git a/field-testing/trackers/dynamic.py b/field-testing/trackers/dynamic.py index 8bb38345..e12f22ad 100644 --- a/field-testing/trackers/dynamic.py +++ b/field-testing/trackers/dynamic.py @@ -2,16 +2,15 @@ # # SPDX-License-Identifier: GPL-2.0-only -import olympe -from olympe.messages.ardrone3.Piloting import PCMD -from olympe.messages.ardrone3.PilotingState import AltitudeChanged -from olympe.messages.gimbal import set_target, attitude, set_max_speed -from olympe.enums.gimbal import control_mode +import json import threading import time -import zmq -import json + import numpy as np +import zmq +from olympe.messages.ardrone3.Piloting import PCMD +from olympe.messages.ardrone3.PilotingState import AltitudeChanged +from olympe.messages.gimbal import attitude, set_max_speed from scipy.spatial.transform import Rotation as R @@ -70,7 +69,7 @@ def calculate_offsets(self, box): drone_roll, drone_pitch = self.get_movement_vectors(target_yaw_angle, target_pitch_angle) - if self.hysteresis and self.prev_center_ts != None and round(time.time() * 1000) - self.prev_center_ts < 500: + if self.hysteresis and self.prev_center_ts is not None and round(time.time() * 1000) - self.prev_center_ts < 500: hysteresis_yaw_angle = ((self.prev_center[0] - target_x_pix) / self.prev_center[0]) * (self.HFOV / 2) hysteresis_pitch_angle = ((self.prev_center[1] - target_y_pix) / self.prev_center[1]) * (self.VFOV / 2) target_yaw_angle += 0.90 * hysteresis_yaw_angle @@ -109,7 +108,7 @@ def run(self): det = json.loads(self.sub_socket.recv_json()) if len(det) > 0: if not self.tracking: - print("Starting new track on object: \"{0}\"".format(det[0]["class"])) + print("Starting new track on object: \"{}\"".format(det[0]["class"])) else: print(f"Got detection from the cloudlet: {det}") self.tracking = True diff --git a/field-testing/trackers/parrot.py b/field-testing/trackers/parrot.py index 97fb04f8..c2e9a7dd 100644 --- a/field-testing/trackers/parrot.py +++ b/field-testing/trackers/parrot.py @@ -2,18 +2,19 @@ # # SPDX-License-Identifier: GPL-2.0-only -import olympe -from olympe.messages.ardrone3.Piloting import TakeOff, moveBy, Landing, moveTo, NavigateHome, PCMD -from olympe.messages.ardrone3.PilotingState import AttitudeChanged, GpsLocationChanged, AltitudeChanged -import olympe.messages.follow_me as follow_me -from olympe.enums.follow_me import mode -from olympe.messages.gimbal import set_target, attitude -from olympe.enums.gimbal import control_mode +import json import threading import time + +import numpy as np +import olympe.messages.follow_me as follow_me import zmq -import json from geopy.distance import geodesic as GD +from olympe.enums.follow_me import mode +from olympe.messages.ardrone3.PilotingState import ( + AltitudeChanged, + GpsLocationChanged, +) class ParrotFollowMeTracker(threading.Thread): @@ -27,8 +28,8 @@ def __init__(self, drone, mode=mode.look_at): super().__init__() def calculate_azimuth_elevation(self, target_lat, target_lon): - gps = drone.get_state(GpsLocationChanged) - alt = drone.get_state(AltitudeChanged) + gps = self.drone.get_state(GpsLocationChanged) + alt = self.drone.get_state(AltitudeChanged) # Elevation calculation d = GD((target_lat, target_lon), (gps["latitude"], gps["longitude"])) @@ -56,7 +57,7 @@ def run(self): try: det = json.loads(self.sub_socket.recv_json()) if not self.tracking and len(det) > 0: - print("Starting new track on object: \"{0}\"".format(det[0]["class"])) + print("Starting new track on object: \"{}\"".format(det[0]["class"])) self.tracking = True azi, elev = self.calculate_azimuth_elevation(det[0]["lat"], det[0]["lon"]) conf = int(det[0]["score"] * 255) @@ -66,7 +67,7 @@ def run(self): self.drone(follow_me.target_framing_position(50, 50)) self.drone(follow_me.start(self.behavior, _no_expect=True)) elif self.tracking and len(det) > 0: - print("Got detection from cloudlet: {0}".format(json.dumps(det))) + print(f"Got detection from cloudlet: {json.dumps(det)}") azi, elev = self.calculate_azimuth_elevation(det[0]["lat"], det[0]["lon"]) conf = int(det[0]["score"] * 255) self.drone(follow_me.target_image_detection(azi, elev, 0.0, conf, 0, self.current_time_millis() - self.start)) diff --git a/field-testing/trackers/static.py b/field-testing/trackers/static.py index 6b1d8d16..7afdaeaf 100644 --- a/field-testing/trackers/static.py +++ b/field-testing/trackers/static.py @@ -2,16 +2,16 @@ # # SPDX-License-Identifier: GPL-2.0-only -import olympe -from olympe.messages.ardrone3.Piloting import PCMD -from olympe.messages.ardrone3.PilotingState import AltitudeChanged -from olympe.messages.gimbal import set_target, attitude -from olympe.enums.gimbal import control_mode -import numpy as np +import json import threading import time + +import numpy as np import zmq -import json +from olympe.enums.gimbal import control_mode +from olympe.messages.ardrone3.Piloting import PCMD +from olympe.messages.ardrone3.PilotingState import AltitudeChanged +from olympe.messages.gimbal import set_target class StaticLeashTracker(threading.Thread): @@ -88,7 +88,7 @@ def run(self): det = json.loads(self.sub_socket.recv_json()) if len(det) > 0: if not self.tracking: - print("Starting new track on object: \"{0}\"".format(det[0]["class"])) + print("Starting new track on object: \"{}\"".format(det[0]["class"])) else: print(f"Got detection from the cloudlet: {det}") self.tracking = True diff --git a/onboard/python/implementation/cloudlets/PartialOffloadCloudlet.py b/onboard/python/implementation/cloudlets/PartialOffloadCloudlet.py index 4228aa73..30382c15 100644 --- a/onboard/python/implementation/cloudlets/PartialOffloadCloudlet.py +++ b/onboard/python/implementation/cloudlets/PartialOffloadCloudlet.py @@ -2,18 +2,16 @@ # # SPDX-License-Identifier: GPL-2.0-only -from interfaces import CloudletItf +import asyncio import json -from json import JSONDecodeError -import threading -import time import logging -import asyncio -import cv2 +from json import JSONDecodeError +import cv2 from cnc_protocol import cnc_pb2 -from gabriel_protocol import gabriel_pb2 from gabriel_client.websocket_client import ProducerWrapper +from gabriel_protocol import gabriel_pb2 +from interfaces import CloudletItf logger = logging.getLogger(__name__) logger.setLevel(logging.DEBUG) @@ -41,7 +39,7 @@ def processResults(self, result_wrapper): data = json.loads(payload) producer = result_wrapper.result_producer_name.value self.engine_results[producer] = result - except JSONDecodeError as e: + except JSONDecodeError: logger.error(f'Error decoding json: {payload}') except Exception as e: print(e) @@ -83,7 +81,7 @@ async def producer(): input_frame.extras.Pack(extras) except Exception as e: input_frame.payload_type = gabriel_pb2.PayloadType.TEXT - input_frame.payloads.append("Unable to produce a frame!".encode('utf-8')) + input_frame.payloads.append(b"Unable to produce a frame!") logger.error(f'Unable to produce a frame: {e}') else: input_frame.payload_type = gabriel_pb2.PayloadType.TEXT diff --git a/onboard/python/implementation/cloudlets/PureOffloadCloudlet.py b/onboard/python/implementation/cloudlets/PureOffloadCloudlet.py index 9c44c44d..4d78a1aa 100644 --- a/onboard/python/implementation/cloudlets/PureOffloadCloudlet.py +++ b/onboard/python/implementation/cloudlets/PureOffloadCloudlet.py @@ -2,20 +2,17 @@ # # SPDX-License-Identifier: GPL-2.0-only -from interfaces import CloudletItf +import asyncio import json -from json import JSONDecodeError -import threading -import time import logging -import asyncio -from syncer import sync -import cv2 -from typing import Tuple +from json import JSONDecodeError +import cv2 from cnc_protocol import cnc_pb2 -from gabriel_protocol import gabriel_pb2 from gabriel_client.websocket_client import ProducerWrapper +from gabriel_protocol import gabriel_pb2 +from interfaces import CloudletItf +from syncer import sync logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -45,7 +42,7 @@ def processResults(self, result_wrapper): data = json.loads(payload) producer = result_wrapper.result_producer_name.value self.engine_results[producer] = result - except JSONDecodeError as e: + except JSONDecodeError: logger.debug(f'Error decoding json: {payload}') except Exception as e: print(e) @@ -64,7 +61,7 @@ def stopStreaming(self): def switchModel(self, model): self.model = model - def setHSVFilter(self, lower_bound: Tuple[int, int, int], upper_bound: Tuple[int, int, int]): + def setHSVFilter(self, lower_bound: tuple[int, int, int], upper_bound: tuple[int, int, int]): self.hsv_lower = lower_bound self.hsv_upper = upper_bound @@ -98,7 +95,7 @@ async def producer(): input_frame.extras.Pack(extras) except Exception as e: input_frame.payload_type = gabriel_pb2.PayloadType.TEXT - input_frame.payloads.append("Unable to produce a frame!".encode('utf-8')) + input_frame.payloads.append(b"Unable to produce a frame!") logger.debug(f'Unable to produce a frame: {e}') else: input_frame.payload_type = gabriel_pb2.PayloadType.TEXT diff --git a/onboard/python/implementation/drones/MavlinkDrone.py b/onboard/python/implementation/drones/MavlinkDrone.py index 791e4b38..71a232c9 100644 --- a/onboard/python/implementation/drones/MavlinkDrone.py +++ b/onboard/python/implementation/drones/MavlinkDrone.py @@ -3,14 +3,22 @@ # SPDX-License-Identifier: GPL-2.0-only import asyncio -from interfaces import DroneItf +import logging import math -from mavsdk import System -from mavsdk.offboard import (OffboardError, PositionNedYaw, VelocityBodyYawspeed, PositionGlobalYaw) +import math as m +import os +import threading import time + +import cv2 import numpy as np -import math as m -import logging +from interfaces import DroneItf +from mavsdk import System +from mavsdk.offboard import ( + OffboardError, + PositionGlobalYaw, + VelocityBodyYawspeed, +) logger = logging.getLogger() @@ -98,10 +106,7 @@ async def connect(self): async def isConnected(self): async for state in self.drone.core.connection_state(): - if state.is_connected: - return True - else: - return False + return bool(state.is_connected) async def disconnect(self): await self.drone.action.disarm() @@ -129,19 +134,19 @@ async def takeOff(self): await self.drone.offboard.set_velocity_body(VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0)) try: await self.drone.offboard.start() - except Exception as e: + except Exception: await self.land() async def land(self): try: await self.drone.offboard.stop() - except OffboardError as error: + except OffboardError: pass await self.drone.action.land() await self.drone.action.disarm() async def setHome(self, lat, lng, alt): - raise NotImplemented() + raise NotImplementedError() async def rth(self): await self.drone.action.set_return_to_launch_altitude(self.RTH_ALT) @@ -185,10 +190,10 @@ async def hover(self): ''' Photography methods ''' async def takePhoto(self): - raise NotImplemented() + raise NotImplementedError() async def toggleThermal(self, on): - raise NotImplemented() + raise NotImplementedError() ''' Status methods ''' @@ -233,11 +238,6 @@ async def kill(self): self.active = False -import cv2 -import numpy as np -import os -import threading - class StreamingThread(threading.Thread): def __init__(self, drone, ip): @@ -259,7 +259,7 @@ def grabFrame(self): try: frame = self.currentFrame.copy() return frame - except Exception as e: + except Exception: # Send a blank frame return np.zeros((720, 1280, 3), np.uint8) diff --git a/onboard/python/implementation/drones/ModalAISeekerDrone.py b/onboard/python/implementation/drones/ModalAISeekerDrone.py index b40fa2b8..c9d5c163 100644 --- a/onboard/python/implementation/drones/ModalAISeekerDrone.py +++ b/onboard/python/implementation/drones/ModalAISeekerDrone.py @@ -3,14 +3,23 @@ # SPDX-License-Identifier: GPL-2.0-only import asyncio -from interfaces import DroneItf +import logging import math -from mavsdk import System -from mavsdk.offboard import (OffboardError, PositionNedYaw, VelocityBodyYawspeed, PositionGlobalYaw) +import math as m +import os +import threading import time + +import cv2 import numpy as np -import math as m -import logging +from interfaces import DroneItf +from mavsdk import System +from mavsdk.offboard import ( + OffboardError, + PositionGlobalYaw, + PositionNedYaw, + VelocityBodyYawspeed, +) logger = logging.getLogger(__name__) logging.basicConfig() @@ -104,10 +113,7 @@ async def connect(self): async def isConnected(self): async for state in self.drone.core.connection_state(): - if state.is_connected: - return True - else: - return False + return bool(state.is_connected) async def disconnect(self): await self.drone.action.disarm() @@ -157,13 +163,13 @@ async def takeOff(self): async def land(self): try: await self.drone.offboard.stop() - except OffboardError as error: + except OffboardError: pass await self.drone.action.land() await self.drone.action.disarm() async def setHome(self, lat, lng, alt): - raise NotImplemented() + raise NotImplementedError() async def rth(self): await self.drone.action.set_return_to_launch_altitude(self.RTH_ALT) @@ -224,10 +230,10 @@ async def hover(self): ''' Photography methods ''' async def takePhoto(self): - raise NotImplemented() + raise NotImplementedError() async def toggleThermal(self, on): - raise NotImplemented() + raise NotImplementedError() ''' Status methods ''' @@ -268,11 +274,6 @@ async def kill(self): self.active = False -import cv2 -import numpy as np -import os -import threading - class StreamingThread(threading.Thread): def __init__(self, drone, ip): @@ -298,7 +299,7 @@ def grabFrame(self): try: frame = self.currentFrame.copy() return frame - except Exception as e: + except Exception: # Send a blank frame return np.zeros((720, 1280, 3), np.uint8) diff --git a/onboard/python/implementation/drones/ParrotAnafiDrone.py b/onboard/python/implementation/drones/ParrotAnafiDrone.py index 95b8a51f..3780d06e 100644 --- a/onboard/python/implementation/drones/ParrotAnafiDrone.py +++ b/onboard/python/implementation/drones/ParrotAnafiDrone.py @@ -3,25 +3,33 @@ # SPDX-License-Identifier: GPL-2.0-only import asyncio +import logging +import math +import os +import queue import threading -from interfaces import DroneItf +import time + +import cv2 +import numpy as np import olympe +import olympe.enums.move as move_mode +from interfaces import DroneItf from olympe import Drone -from olympe.messages.ardrone3.Piloting import TakeOff, Landing -from olympe.messages.ardrone3.Piloting import PCMD, moveTo, moveBy -from olympe.messages.rth import set_custom_location, return_to_home -from olympe.messages.ardrone3.PilotingState import moveToChanged -from olympe.messages.common.CommonState import BatteryStateChanged -from olympe.messages.ardrone3.PilotingState import AttitudeChanged, GpsLocationChanged, AltitudeChanged, FlyingStateChanged, SpeedChanged from olympe.messages.ardrone3.GPSState import NumberOfSatelliteChanged -from olympe.messages.gimbal import set_target, attitude -from olympe.messages.wifi import rssi_changed -from olympe.messages.battery import capacity +from olympe.messages.ardrone3.Piloting import PCMD, Landing, TakeOff, moveBy, moveTo +from olympe.messages.ardrone3.PilotingState import ( + AltitudeChanged, + AttitudeChanged, + FlyingStateChanged, + GpsLocationChanged, + SpeedChanged, +) from olympe.messages.common.CalibrationState import MagnetoCalibrationRequiredState -import olympe.enums.move as move_mode -import olympe.enums.gimbal as gimbal_mode -import math -import logging +from olympe.messages.common.CommonState import BatteryStateChanged +from olympe.messages.gimbal import attitude, set_target +from olympe.messages.rth import return_to_home, set_custom_location +from olympe.messages.wifi import rssi_changed logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -54,9 +62,7 @@ async def hovering(self, timeout=None): if timeout is not None: start = time.time() while True: - if self.drone(FlyingStateChanged(state="hovering", _policy="check")).success(): - break - elif start is not None and time.time() - start < timeout: + if self.drone(FlyingStateChanged(state="hovering", _policy="check")).success() or start is not None and time.time() - start < timeout: break else: await asyncio.sleep(1) @@ -224,10 +230,6 @@ async def kill(self): self.active = False -import cv2 -import numpy as np -import os - class StreamingThread(threading.Thread): def __init__(self, drone, ip): @@ -249,14 +251,13 @@ def grabFrame(self): try: frame = self.currentFrame.copy() return frame - except Exception as e: + except Exception: # Send a blank frame return np.zeros((720, 1280, 3), np.uint8) def stop(self): self.isRunning = False -import queue class LowDelayStreamingThread(threading.Thread): @@ -292,7 +293,7 @@ def grabFrame(self): try: frame = self.currentFrame.copy() return frame - except Exception as e: + except Exception: # Send a blank frame return np.zeros((720, 1280, 3), np.uint8) diff --git a/onboard/python/implementation/drones/test.py b/onboard/python/implementation/drones/test.py index 3ffb6627..596e82c3 100644 --- a/onboard/python/implementation/drones/test.py +++ b/onboard/python/implementation/drones/test.py @@ -1,9 +1,9 @@ -from ModalAISeekerDrone import ModalAISeekerDrone import asyncio import logging - import time +from ModalAISeekerDrone import ModalAISeekerDrone + logger = logging.getLogger(__name__) logging.basicConfig() logger.setLevel(logging.INFO) diff --git a/onboard/python/supervisor.py b/onboard/python/supervisor.py index 30a396ad..319b3928 100644 --- a/onboard/python/supervisor.py +++ b/onboard/python/supervisor.py @@ -4,24 +4,25 @@ import argparse import asyncio -import nest_asyncio -nest_asyncio.apply() -from syncer import sync +import importlib import logging -import requests +import os import subprocess import sys -import validators -import os +import time from zipfile import ZipFile -import importlib +#from websocket_client import WebsocketClient +import nest_asyncio +import requests +import validators +import zmq from cnc_protocol import cnc_pb2 -from gabriel_protocol import gabriel_pb2 from gabriel_client.websocket_client import ProducerWrapper, WebsocketClient -#from websocket_client import WebsocketClient +from gabriel_protocol import gabriel_pb2 +from syncer import sync -import zmq +nest_asyncio.apply() logger = logging.getLogger() logger.setLevel(logging.INFO) @@ -44,18 +45,18 @@ def __init__(self, args): cloudlet_import = f"implementation.cloudlets.{args.cloudlet}" try: Drone = importlib.import_module(drone_import) - except Exception as e: + except Exception: logger.info('Could not import drone {args.drone}') sys.exit(0) try: Cloudlet = importlib.import_module(cloudlet_import) - except Exception as e: + except Exception: logger.info('Could not import cloudlet {args.cloudlet}') sys.exit(0) try: self.cloudlet = getattr(Cloudlet, args.cloudlet)() - except Exception as e: + except Exception: logger.info('Could not initialize {args.cloudlet}, name does not exist. Aborting.') sys.exit(0) try: @@ -70,7 +71,7 @@ def __init__(self, args): kwargs['droneip'] = args.droneip logger.info(f"{kwargs=}") self.drone = getattr(Drone, args.drone)(**kwargs) - except Exception as e: + except Exception: logger.info('Could not initialize {args.drone}, name does not exist. Aborting.') sys.exit(0) @@ -96,7 +97,7 @@ async def executeFlightScript(self, url: str): logger.debug('Starting flight plan download...') try: self.download(url) - except Exception as e: + except Exception: logger.debug('Flight script download failed! Aborting.') return logger.debug('Flight script downloaded...') @@ -124,7 +125,7 @@ def start_mission(self): logger.debug('MC init') #from mission.MissionController import MissionController Mission = importlib.import_module(f"{module_prefix}.mission.MissionController") - self.mission = getattr(Mission, "MissionController")(self.drone, self.cloudlet) + self.mission = Mission.MissionController(self.drone, self.cloudlet) logger.debug('Running flight script!') self.missionTask = asyncio.create_task(self.mission.run()) self.reload = True @@ -181,7 +182,7 @@ async def commandHandler(self): try: self.zmq.send(req.SerializeToString()) rep = self.zmq.recv() - if b'No commands.' != rep: + if rep != b'No commands.': extras = cnc_pb2.Extras() extras.ParseFromString(rep) if extras.cmd.rth: @@ -206,13 +207,13 @@ async def commandHandler(self): logger.info(f'Invalid script URL sent by commander: {extras.cmd.script_url}') elif self.manual: if extras.cmd.takeoff: - logger.info(f'Received manual takeoff') + logger.info('Received manual takeoff') asyncio.create_task(self.drone.takeOff()) elif extras.cmd.land: - logger.info(f'Received manual land') + logger.info('Received manual land') asyncio.create_task(self.drone.land()) else: - logger.info(f'Received manual PCMD') + logger.info('Received manual PCMD') pitch = extras.cmd.pcmd.pitch yaw = extras.cmd.pcmd.yaw roll = extras.cmd.pcmd.roll @@ -251,7 +252,7 @@ async def producer(): self.heartbeats += 1 input_frame = gabriel_pb2.InputFrame() input_frame.payload_type = gabriel_pb2.PayloadType.TEXT - input_frame.payloads.append('heartbeart'.encode('utf8')) + input_frame.payloads.append(b'heartbeart') extras = cnc_pb2.Extras() try: diff --git a/os/drivers/ModalAI/Seeker/Seeker.py b/os/drivers/ModalAI/Seeker/Seeker.py index 7ff9cbb9..fd2b91eb 100644 --- a/os/drivers/ModalAI/Seeker/Seeker.py +++ b/os/drivers/ModalAI/Seeker/Seeker.py @@ -1,17 +1,18 @@ -from enum import Enum +import asyncio +import logging import math import os +import threading import time -import asyncio -import logging +from enum import Enum + +import cv2 from pymavlink import mavutil logger = logging.getLogger(__name__) -class ConnectionFailedException(Exception): - pass -class ModalAISeekerDrone(): +class ModalAISeekerDrone: class FlightMode(Enum): LAND = 'LAND' @@ -275,7 +276,7 @@ async def setHome(self, lat, lng, alt): async def rth(self): logger.info("-- Returning to launch") - if await self.switchMode(ModalAISeekerDrone.FlightMode.RTL) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.RTL) is False: logger.error("Failed to set mode to RTL") return @@ -291,14 +292,14 @@ async def rth(self): async def manual_control(self, forward_vel, right_vel, up_vel, angle_vel): if self.gps_disabled: - if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) is False: logger.error("Failed to set mode to GUIDED_NOGPS") return # if await self.switchMode(ModalAISeekerDrone.FlightMode.ALT_HOLD) == False: # logger.error("Failed to set mode to GUIDED_NOGPS") # return else: - if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) is False: logger.error("Failed to set mode to GUIDED") return logger.info(f"Sending manual control: forward={forward_vel}, right={right_vel}, up={up_vel}, yaw={angle_vel}") @@ -335,14 +336,14 @@ async def setAttitude(self, pitch, roll, thrust, yaw): logger.info(f"-- Setting attitude: pitch={pitch}, roll={roll}, thrust={thrust}, yaw={yaw}") if self.gps_disabled: - if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) is False: logger.error("Failed to set mode to GUIDED_NOGPS") return # if await self.switchMode(ModalAISeekerDrone.FlightMode.ALT_HOLD) == False: # logger.error("Failed to set mode to GUIDED_NOGPS") # return else: - if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) is False: logger.error("Failed to set mode to GUIDED") return @@ -377,7 +378,7 @@ def to_quaternion(roll=0.0, pitch=0.0, yaw=0.0): async def setVelocity(self, forward_vel, right_vel, up_vel, angle_vel): logger.info(f"-- Setting velocity: forward_vel={forward_vel}, right_vel={right_vel}, up_vel={up_vel}, angle_vel={angle_vel}") - if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) is False: logger.error("Failed to set mode to GUIDED") return @@ -397,7 +398,7 @@ async def setVelocity(self, forward_vel, right_vel, up_vel, angle_vel): async def setGPSLocation(self, lat, lon, alt, bearing): logger.info(f"-- Setting GPS location: lat={lat}, lon={lon}, alt={alt}, bearing={bearing}") - if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) is False: logger.error("Failed to set mode to GUIDED") return @@ -439,7 +440,7 @@ async def setGPSLocation(self, lat, lon, alt, bearing): async def setTranslatedLocation(self, forward, right, up, angle): logger.info(f"-- Translating location: forward={forward}, right={right}, up={up}, angle={angle}") - if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) is False: logger.error("Failed to set mode to GUIDED") return @@ -485,7 +486,7 @@ async def setTranslatedLocation(self, forward, right, up, angle): async def setBearing(self, bearing): logger.info(f"-- Setting yaw to {bearing} degrees") - if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) == False: + if await self.switchMode(ModalAISeekerDrone.FlightMode.OFFBOARD) is False: logger.error("Failed to set mode to GUIDED") return @@ -603,7 +604,7 @@ async def switchMode(self, mode): return result else: - logger.info(f"Priming for OFFBOARD mode") + logger.info("Priming for OFFBOARD mode") return True async def disableGPS(self): @@ -745,10 +746,6 @@ async def getVideoFrame(self): async def stopStreaming(self): self.streamingThread.stop() -import cv2 -import numpy as np -import os -import threading class StreamingThread(threading.Thread): @@ -760,10 +757,7 @@ def __init__(self, drone): url_mini = os.environ.get('STREAM_MINI_URL') self.sim = os.environ.get('SIMULATION') - if (self.sim == 'true'): - url = url_sim - else: - url = url_mini + url = url_sim if self.sim == 'true' else url_mini self.cap = cv2.VideoCapture(url) self.isRunning = True @@ -782,7 +776,7 @@ def grabFrame(self): try: frame = self.currentFrame.copy() return frame - except Exception as e: + except Exception: # Send a blank frame return None diff --git a/os/drivers/Parrot/ParrotAnafi/ParrotAnafi.py b/os/drivers/Parrot/ParrotAnafi/ParrotAnafi.py index ce0e7b23..47634351 100644 --- a/os/drivers/Parrot/ParrotAnafi/ParrotAnafi.py +++ b/os/drivers/Parrot/ParrotAnafi/ParrotAnafi.py @@ -2,35 +2,43 @@ # # SPDX-License-Identifier: GPL-2.0-only -import logging import asyncio -import threading +import logging import math import os +import queue +import threading import time +from enum import Enum +import cv2 +import logness +import numpy as np import olympe +import olympe.enums.move as move_mode from olympe import Drone -from olympe.messages.ardrone3.Piloting import TakeOff, Landing -from olympe.messages.ardrone3.Piloting import PCMD, moveTo, moveBy -from olympe.messages.rth import set_custom_location, return_to_home -from olympe.messages.ardrone3.PilotingState import moveToChanged -from olympe.messages.common.CommonState import BatteryStateChanged -from olympe.messages.ardrone3.PilotingSettingsState import MaxTiltChanged -from olympe.messages.ardrone3.SpeedSettingsState import MaxVerticalSpeedChanged, MaxRotationSpeedChanged -from olympe.messages.ardrone3.PilotingState import AttitudeChanged, GpsLocationChanged, AltitudeChanged, FlyingStateChanged, SpeedChanged from olympe.messages.ardrone3.GPSState import NumberOfSatelliteChanged -from olympe.messages.gimbal import set_target, attitude -from olympe.messages.wifi import rssi_changed -from olympe.messages.battery import capacity +from olympe.messages.ardrone3.Piloting import PCMD, Landing, TakeOff, moveBy, moveTo +from olympe.messages.ardrone3.PilotingSettingsState import MaxTiltChanged +from olympe.messages.ardrone3.PilotingState import ( + AltitudeChanged, + AttitudeChanged, + FlyingStateChanged, + GpsLocationChanged, + SpeedChanged, +) +from olympe.messages.ardrone3.SpeedSettingsState import ( + MaxRotationSpeedChanged, + MaxVerticalSpeedChanged, +) from olympe.messages.common.CalibrationState import MagnetoCalibrationRequiredState -import olympe.enums.move as move_mode -import olympe.enums.gimbal as gimbal_mode -from enum import Enum +from olympe.messages.common.CommonState import BatteryStateChanged +from olympe.messages.gimbal import attitude, set_target +from olympe.messages.rth import return_to_home, set_custom_location +from olympe.messages.wifi import rssi_changed logger = logging.getLogger(__name__) -import logness logness.update_config({ "handlers": { "olympe_log_file": { @@ -62,7 +70,7 @@ class ArgumentOutOfBoundsException(Exception): class ConnectionFailedException(Exception): pass -class ParrotDrone(): +class ParrotDrone: class FlightMode(Enum): MANUAL = 1 @@ -112,9 +120,7 @@ async def hovering(self, timeout=None): if timeout is not None: start = time.time() while True: - if self.drone(FlyingStateChanged(state="hovering", _policy="check")).success(): - break - elif start is not None and time.time() - start < timeout: + if self.drone(FlyingStateChanged(state="hovering", _policy="check")).success() or start is not None and time.time() - start < timeout: break else: await asyncio.sleep(1) @@ -145,10 +151,7 @@ def updatePID(e, ep, tp, ts, pidDict): I *= -1 elif abs(e) <= 0.01 or I * pidDict["PrevI"] < 0: I = 0.0 - if abs(e) > 0.01: - D = pidDict["Kd"] * (e - ep) / (ts - tp) - else: - D = 0 + D = pidDict["Kd"] * (e - ep) / (ts - tp) if abs(e) > 0.01 else 0 return P, I, D @@ -234,10 +237,7 @@ def updatePID(e, ep, tp, ts, pidDict): I *= -1 elif abs(e) <= 0.05 or I * pidDict["PrevI"] < 0: I = 0.0 - if abs(e) > 0.01: - D = pidDict["Kd"] * (e - ep) / (ts - tp) - else: - D = 0.0 + D = pidDict["Kd"] * (e - ep) / (ts - tp) if abs(e) > 0.01 else 0.0 # For testing Integral component I = 0.0 @@ -492,7 +492,7 @@ async def getGPS(self): return (self.drone.get_state(GpsLocationChanged)["latitude"], self.drone.get_state(GpsLocationChanged)["longitude"], self.drone.get_state(GpsLocationChanged)["altitude"]) - except Exception as e: + except Exception: # If there is no GPS fix, return default values return (500.0, 500.0, 0.0) @@ -560,10 +560,6 @@ async def kill(self): self.active = False -import cv2 -import numpy as np -import os - class FFMPEGStreamingThread(threading.Thread): def __init__(self, drone, ip): @@ -594,7 +590,6 @@ def grabFrame(self): def stop(self): self.isRunning = False -import queue class PDRAWStreamingThread(threading.Thread): diff --git a/os/drivers/SkyRocket/SkyViper2450GPS/SkyViper2450GPS.py b/os/drivers/SkyRocket/SkyViper2450GPS/SkyViper2450GPS.py index e820a032..0f5e62a4 100644 --- a/os/drivers/SkyRocket/SkyViper2450GPS/SkyViper2450GPS.py +++ b/os/drivers/SkyRocket/SkyViper2450GPS/SkyViper2450GPS.py @@ -1,17 +1,18 @@ -from enum import Enum +import asyncio +import logging import math import os +import threading import time -import asyncio -import logging +from enum import Enum + +import cv2 from pymavlink import mavutil logger = logging.getLogger(__name__) -class ConnectionFailedException(Exception): - pass -class SkyViper2450GPSDrone(): +class SkyViper2450GPSDrone: class FlightMode(Enum): LAND = 'LAND' @@ -209,11 +210,11 @@ async def hover(self): async def takeOff(self, target_altitude): logger.info("-- Taking off") - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) is False: logger.error("Failed to set mode to GUIDED") return - if await self.arm() == False: + if await self.arm() is False: logger.error("Failed to arm the drone") return @@ -240,7 +241,7 @@ async def takeOff(self, target_altitude): async def land(self): logger.info("-- Landing") - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.LAND) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.LAND) is False: logger.error("Failed to set mode to LAND") return @@ -282,7 +283,7 @@ async def setHome(self, lat, lng, alt): async def rth(self): logger.info("-- Returning to launch") - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.RTL) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.RTL) is False: logger.error("Failed to set mode to RTL") return @@ -299,14 +300,14 @@ async def rth(self): async def manual_control(self, forward_vel, right_vel, up_vel, angle_vel): if self.gps_disabled: - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED_NOGPS) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED_NOGPS) is False: logger.error("Failed to set mode to GUIDED_NOGPS") return # if await self.switchMode(SkyViper2450GPSDrone.FlightMode.ALT_HOLD) == False: # logger.error("Failed to set mode to GUIDED_NOGPS") # return else: - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) is False: logger.error("Failed to set mode to GUIDED") return logger.info(f"Sending manual control: forward={forward_vel}, right={right_vel}, up={up_vel}, yaw={angle_vel}") @@ -343,14 +344,14 @@ async def setAttitude(self, pitch, roll, thrust, yaw): logger.info(f"-- Setting attitude: pitch={pitch}, roll={roll}, thrust={thrust}, yaw={yaw}") if self.gps_disabled: - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED_NOGPS) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED_NOGPS) is False: logger.error("Failed to set mode to GUIDED_NOGPS") return # if await self.switchMode(SkyViper2450GPSDrone.FlightMode.ALT_HOLD) == False: # logger.error("Failed to set mode to GUIDED_NOGPS") # return else: - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) is False: logger.error("Failed to set mode to GUIDED") return @@ -386,11 +387,11 @@ async def setVelocity(self, forward_vel, right_vel, up_vel, angle_vel): logger.info(f"-- Setting velocity: forward_vel={forward_vel}, right_vel={right_vel}, up_vel={up_vel}, angle_vel={angle_vel}") if self.gps_disabled: - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED_NOGPS) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED_NOGPS) is False: logger.error("Failed to set mode to GUIDED_NOGPS") return else: - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) is False: logger.error("Failed to set mode to GUIDED") return @@ -411,7 +412,7 @@ async def setVelocity(self, forward_vel, right_vel, up_vel, angle_vel): async def setGPSLocation(self, lat, lon, alt, bearing): logger.info(f"-- Setting GPS location: lat={lat}, lon={lon}, alt={alt}, bearing={bearing}") - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) is False: logger.error("Failed to set mode to GUIDED") return @@ -454,7 +455,7 @@ async def setGPSLocation(self, lat, lon, alt, bearing): async def setTranslatedLocation(self, forward, right, up, angle): logger.info(f"-- Translating location: forward={forward}, right={right}, up={up}, angle={angle}") - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) is False: logger.error("Failed to set mode to GUIDED") return @@ -501,7 +502,7 @@ async def setTranslatedLocation(self, forward, right, up, angle): async def setBearing(self, bearing): logger.info(f"-- Setting yaw to {bearing} degrees") - if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) == False: + if await self.switchMode(SkyViper2450GPSDrone.FlightMode.GUIDED) is False: logger.error("Failed to set mode to GUIDED") return @@ -757,10 +758,6 @@ async def getVideoFrame(self): async def stopStreaming(self): self.streamingThread.stop() -import cv2 -import numpy as np -import os -import threading class StreamingThread(threading.Thread): @@ -772,10 +769,7 @@ def __init__(self, drone): url_mini = os.environ.get('STREAM_MINI_URL') self.sim = os.environ.get('SIMULATION') - if (self.sim == 'true'): - url = url_sim - else: - url = url_mini + url = url_sim if self.sim == 'true' else url_mini logger.info(f"url used: {url}") self.cap = cv2.VideoCapture(url) @@ -798,7 +792,7 @@ def grabFrame(self): try: frame = self.currentFrame.copy() return frame - except Exception as e: + except Exception: # Send a blank frame return None diff --git a/os/drivers/base/DroneItf.py b/os/drivers/base/DroneItf.py index 4acb3ee9..b463efd0 100644 --- a/os/drivers/base/DroneItf.py +++ b/os/drivers/base/DroneItf.py @@ -1,5 +1,6 @@ -from abs import ABC -import asyncio + +from abs import ABC, abstractmethod + class DroneDeviceItf(ABC): """ @@ -27,7 +28,7 @@ def __bool__(self): """ Overloaded boolean operator to support easy success checks. """ - return rid != 0 + return self.rid != 0 @abstractmethod async def connect(self): @@ -168,12 +169,12 @@ async def setRelativePosition(self, north, east, up, bearing): """ pass - async def hover(self): - """ - Instruct the drone to hover. - - :return: Response object - :rtype: class: Response - """ - pass + async def hover(self): + """ + Instruct the drone to hover. + + :return: Response object + :rtype: class: Response + """ + pass diff --git a/os/drivers/base/common.py b/os/drivers/base/common.py new file mode 100644 index 00000000..b7d2e68e --- /dev/null +++ b/os/drivers/base/common.py @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: 2025 Carnegie Mellon University - Satyalab +# +# SPDX-License-Identifier: GPL-2.0-only + +class ConnectionFailedException(Exception): + pass diff --git a/os/drivers/driver.py b/os/drivers/driver.py index 2878e597..cd032b51 100644 --- a/os/drivers/driver.py +++ b/os/drivers/driver.py @@ -1,16 +1,18 @@ -import time -import zmq -import zmq.asyncio +import asyncio import json +import logging import os +import signal import sys -import asyncio -import logging +import time + import cnc_protocol.cnc_pb2 as cnc_protocol -from util.utils import setup_socket, SocketOperation -import signal -from drivers.ModalAI.Seeker.Seeker import ModalAISeekerDrone, ConnectionFailedException -from drivers.SkyRocket.SkyViper2450GPS.SkyViper2450GPS import SkyViper2450GPSDrone, ConnectionFailedException +import zmq +import zmq.asyncio +from drivers.base.common import ConnectionFailedException +from drivers.ModalAI.Seeker.Seeker import ModalAISeekerDrone +from drivers.SkyRocket.SkyViper2450GPS.SkyViper2450GPS import SkyViper2450GPSDrone +from util.utils import SocketOperation, setup_socket # Configure logger logging_format = '%(asctime)s - %(levelname)s - %(name)s - %(message)s' @@ -182,10 +184,10 @@ async def main(drone, cam_sock, tel_sock, args): logger.info('starting connecting...') await drone.connect(connection_string) logger.info('drone connected') - except ConnectionFailedException as e: + except ConnectionFailedException: logger.error('Failed to connect to drone, retrying...') continue - logger.info(f'Established connection to drone, ready to receive commands!') + logger.info('Established connection to drone, ready to receive commands!') await drone.startStreaming() logger.info('Started streaming') @@ -213,7 +215,7 @@ async def main(drone, cam_sock, tel_sock, args): except Exception as e: logger.info(f'cmd received error: {e}') - logger.info(f"Disconnected from drone") + logger.info("Disconnected from drone") if __name__ == "__main__": asyncio.run(main(drone, cam_sock, tel_sock, driverArgs)) diff --git a/os/drivers/test/driver_test.py b/os/drivers/test/driver_test.py index 74242ad4..8598b9d7 100644 --- a/os/drivers/test/driver_test.py +++ b/os/drivers/test/driver_test.py @@ -1,14 +1,15 @@ -from pynput.keyboard import Listener, Key, KeyCode -from enum import Enum -import subprocess +import asyncio import logging import time -import zmq -import zmq.asyncio -import asyncio from collections import defaultdict -from util.utils import setup_socket, SocketOperation +from enum import Enum + import cnc_protocol.cnc_pb2 as cnc_pb2 +import zmq +import zmq.asyncio +from pynput.keyboard import Key, KeyCode, Listener +from util.utils import SocketOperation, setup_socket + class Ctrl(Enum): ( @@ -52,10 +53,7 @@ def _on_press(self, key): self._key_pressed[key.char] = True elif isinstance(key, Key): self._key_pressed[key] = True - if self._key_pressed[self._ctrl_keys[Ctrl.QUIT]]: - return False - else: - return True + return not self._key_pressed[self._ctrl_keys[Ctrl.QUIT]] def _on_release(self, key): if isinstance(key, KeyCode): diff --git a/os/kernel/CommandService.py b/os/kernel/CommandService.py index eb65c062..1924cb88 100644 --- a/os/kernel/CommandService.py +++ b/os/kernel/CommandService.py @@ -1,15 +1,15 @@ -from enum import Enum -import sys +import asyncio +import logging +import os import time +from enum import Enum + import validators import zmq import zmq.asyncio -import asyncio -import logging -import os from cnc_protocol import cnc_pb2 from kernel.Service import Service -from util.utils import setup_socket, SocketOperation +from util.utils import SocketOperation, setup_socket # Configure logger logging.basicConfig(level=os.environ.get('LOG_LEVEL', logging.INFO), @@ -178,13 +178,13 @@ async def cmd_proxy(self): logger.debug(f"proxy : cmd_back_sock Received message from BACKEND: identity: {identity} cmd: {cmd}") if identity == b'cmdr': - logger.debug(f"proxy : cmd_back_sock Received message from BACKEND: discard bc of cmdr") + logger.debug("proxy : cmd_back_sock Received message from BACKEND: discard bc of cmdr") pass elif identity == b'usr': - logger.debug(f"proxy : cmd_back_sock Received message from BACKEND: sent back bc of user") + logger.debug("proxy : cmd_back_sock Received message from BACKEND: sent back bc of user") await self.cmd_front_usr_sock.send_multipart([cmd]) else: - logger.error(f"proxy: invalid identity") + logger.error("proxy: invalid identity") except Exception as e: logger.error(f"proxy: {e}") diff --git a/os/kernel/DataService.py b/os/kernel/DataService.py index 161c79d5..a92684ac 100644 --- a/os/kernel/DataService.py +++ b/os/kernel/DataService.py @@ -1,19 +1,19 @@ -import time -import zmq -import zmq.asyncio import asyncio -import os -import logging -import yaml import importlib +import logging +import os import pkgutil -from util.utils import setup_socket, SocketOperation -from cnc_protocol import cnc_pb2 +import sys + import computes -from kernel.computes.ComputeItf import ComputeInterface +import yaml +import zmq +import zmq.asyncio +from cnc_protocol import cnc_pb2 from DataStore import DataStore +from kernel.computes.ComputeItf import ComputeInterface from kernel.Service import Service -import sys +from util.utils import SocketOperation, setup_socket # Set up logging logging_format = '%(asctime)s - %(levelname)s - %(name)s - %(message)s' @@ -79,7 +79,7 @@ def __init__(self, config_yaml): def get_result(self, compute_type): logger.info(f"Processing getter for compute type: {compute_type}") getter_list = [] - for compute_id in self.compute_dict.keys(): + for compute_id in self.compute_dict: cpt_res = self.data_store.get_compute_result(compute_id, compute_type) if cpt_res is None: @@ -100,7 +100,7 @@ def get_result(self, compute_type): def clear_result(self): logger.info("Processing setter") - for compute_id in self.compute_dict.keys(): + for compute_id in self.compute_dict: self.data_store.clear_compute_result(compute_id) async def user_handler(self): diff --git a/os/kernel/DataStore.py b/os/kernel/DataStore.py index f07e6906..c3120d00 100644 --- a/os/kernel/DataStore.py +++ b/os/kernel/DataStore.py @@ -1,8 +1,8 @@ import asyncio -from cnc_protocol import cnc_pb2 -from typing import Optional, Union import logging +from cnc_protocol import cnc_pb2 + logger = logging.getLogger(__name__) class DataStore: @@ -30,7 +30,7 @@ def clear_compute_result(self, compute_id): self._result_cache.pop(compute_id, None) ######################################################## COMPUTE ############################################################ - def get_compute_result(self, compute_id, result_type: str) -> Optional[Union[None, tuple]]: + def get_compute_result(self, compute_id, result_type: str) -> tuple | None: logger.info(f"get_compute_result: Getting result for compute {compute_id} with type {result_type}") logger.info(self._result_cache) if compute_id not in self._result_cache: diff --git a/os/kernel/Service.py b/os/kernel/Service.py index 19829191..e3a45f2d 100644 --- a/os/kernel/Service.py +++ b/os/kernel/Service.py @@ -21,7 +21,7 @@ def register_task(self, task): self.tasks.append(task) async def start(self): - logger.info(f'service started') + logger.info('service started') try: await asyncio.gather(*self.tasks) diff --git a/os/kernel/computes/GabrielCompute.py b/os/kernel/computes/GabrielCompute.py index 8b895fd7..d58036a5 100644 --- a/os/kernel/computes/GabrielCompute.py +++ b/os/kernel/computes/GabrielCompute.py @@ -1,14 +1,13 @@ import asyncio -import json import logging import os import time + import cv2 import numpy as np -from gabriel_protocol import gabriel_pb2 -from gabriel_client.zeromq_client import ProducerWrapper, ZeroMQClient -from util.timer import Timer from cnc_protocol import cnc_pb2 +from gabriel_client.zeromq_client import ProducerWrapper, ZeroMQClient +from gabriel_protocol import gabriel_pb2 from kernel.computes.ComputeItf import ComputeInterface from kernel.DataStore import DataStore @@ -45,7 +44,7 @@ def __init__(self, compute_id, data_store:DataStore): self.frame_id = -1 async def run(self): - logger.info(f"Gabriel compute: launching Gabriel client") + logger.info("Gabriel compute: launching Gabriel client") await self.gabriel_client.launch_async() @@ -138,10 +137,10 @@ async def producer(): else: logger.info('Gabriel compute Frame producer: frame is None') input_frame.payload_type = gabriel_pb2.PayloadType.TEXT - input_frame.payloads.append("Streaming not started, no frame to show.".encode('utf-8')) + input_frame.payloads.append(b"Streaming not started, no frame to show.") except Exception as e: input_frame.payload_type = gabriel_pb2.PayloadType.TEXT - input_frame.payloads.append("Unable to produce a frame!".encode('utf-8')) + input_frame.payloads.append(b"Unable to produce a frame!") logger.error(f'Gabriel compute Frame producer: unable to produce a frame: {e}') logger.debug(f"Gabriel compute Frame producer: finished time {time.time()}") @@ -156,7 +155,7 @@ async def producer(): logger.debug(f"tel producer: starting time {time.time()}") input_frame = gabriel_pb2.InputFrame() input_frame.payload_type = gabriel_pb2.PayloadType.TEXT - input_frame.payloads.append('heartbeart'.encode('utf8')) + input_frame.payloads.append(b'heartbeart') tel_data = cnc_pb2.Telemetry() self.data_store.get_raw_data(tel_data) try: @@ -180,7 +179,7 @@ async def producer(): extras.registering = True self.drone_registered = True - logger.debug('Gabriel compute telemetry producer: sending Gabriel telemerty! content: {}'.format(extras)) + logger.debug(f'Gabriel compute telemetry producer: sending Gabriel telemerty! content: {extras}') input_frame.extras.Pack(extras) else: logger.error('Telemetry unavailable') diff --git a/os/kernel/computes/VOXLCompute.py b/os/kernel/computes/VOXLCompute.py index 6ad1f9ea..ba8c8783 100644 --- a/os/kernel/computes/VOXLCompute.py +++ b/os/kernel/computes/VOXLCompute.py @@ -1,16 +1,17 @@ import asyncio -from cnc_protocol import cnc_pb2 -import cv2 -from enum import Enum -from kernel.computes.ComputeItf import ComputeInterface -from kernel.DataStore import DataStore import logging -import numpy as np import os +from enum import Enum + +import cv2 import kernel.computes.onboard_compute_pb2 as onboard_compute_pb2 -from util.utils import setup_socket, SocketOperation, lazy_pirate_request +import numpy as np import zmq import zmq.asyncio +from cnc_protocol import cnc_pb2 +from kernel.computes.ComputeItf import ComputeInterface +from kernel.DataStore import DataStore +from util.utils import SocketOperation, lazy_pirate_request, setup_socket logger = logging.getLogger(__name__) @@ -61,7 +62,7 @@ async def get_status(self): return super().get_status() async def set(self): - raise NotImplemented() + raise NotImplementedError() async def run_loop(self): ''' @@ -114,11 +115,11 @@ async def process_frame(self, frame: np.ndarray, computation_type: ComputationTy self.socket, request.SerializeToString(), self.context, self.server_endpoint) - if reply == None: - logger.error(f"Local compute engine did not respond to request") + if reply is None: + logger.error("Local compute engine did not respond to request") return - logger.info(f"Received response from local compute engine") + logger.info("Received response from local compute engine") detections = onboard_compute_pb2.ComputeResult() detections.ParseFromString(reply) logger.info(f"Received detections: {detections}") diff --git a/os/test/dummy_commander.py b/os/test/dummy_commander.py index 59bedd7d..0232ddb5 100644 --- a/os/test/dummy_commander.py +++ b/os/test/dummy_commander.py @@ -1,9 +1,10 @@ +import asyncio import os -import zmq import time -from cnc_protocol import cnc_pb2 -import asyncio + +import zmq import zmq.asyncio +from cnc_protocol import cnc_pb2 from util.utils import setup_socket context = zmq.Context() @@ -16,7 +17,7 @@ -class c_client(): +class c_client: def send_takeOff(self): driver_command = cnc_pb2.Extras() driver_command.cmd.takeoff = True @@ -28,7 +29,7 @@ def send_land(self): driver_command = cnc_pb2.Extras() driver_command.cmd.land = True message = driver_command.SerializeToString() - cmd_front_socket.send_multipart([message]) + cmd_front_sock.send_multipart([message]) print(f"commander: land sent at: {time.time()}") def send_MCOM(self, key): diff --git a/os/test/dummy_driver.py b/os/test/dummy_driver.py index cbdfda7c..2ca5856b 100644 --- a/os/test/dummy_driver.py +++ b/os/test/dummy_driver.py @@ -1,6 +1,7 @@ import asyncio import os import time + import zmq import zmq.asyncio from cnc_protocol import cnc_pb2 @@ -26,7 +27,7 @@ async def camera_stream(drone, camera_sock): while True: try: x = 1 - except Exception as e: + except Exception: pass await asyncio.sleep(0.033) @@ -40,7 +41,7 @@ async def telemetry_stream(drone, telemetry_sock): print(f'Failed to get telemetry, error: {e}') await asyncio.sleep(0) -class d_server(): +class d_server: async def a_run(self): asyncio.create_task(telemetry_stream(None, tel_sock)) @@ -84,7 +85,7 @@ async def a_run(self): # Send a reply back to the client with the identity frame and empty delimiter cmd_back_sock.send_multipart([identity, serialized_response]) - print(f"done processing request") + print("done processing request") except Exception as e: print(f"error: {e}") diff --git a/os/test/dummy_kernel_cmd.py b/os/test/dummy_kernel_cmd.py index 926ce9d0..735217eb 100644 --- a/os/test/dummy_kernel_cmd.py +++ b/os/test/dummy_kernel_cmd.py @@ -1,12 +1,13 @@ +import asyncio +import logging import os -import zmq +import sys import time -from cnc_protocol import cnc_pb2 -import asyncio + +import zmq import zmq.asyncio +from cnc_protocol import cnc_pb2 from util.utils import SocketOperation, setup_socket -import logging -import sys # Configure logger logger = logging.getLogger() @@ -49,7 +50,7 @@ setup_socket(msn_sock, SocketOperation.BIND, 'MSN_PORT', 'Created userspace mission control socket endpoint') -class k_client(): +class k_client: # Function to send a start mission command def send_start_mission(self): @@ -104,7 +105,7 @@ async def proxy(self): elif identity == b'usr': await cmd_back_sock.send_multipart(message) else: - print(f"cmd_proxy: invalid identity") + print("cmd_proxy: invalid identity") # Check for messages on the DEALER socket @@ -118,13 +119,13 @@ async def proxy(self): print(f"proxy : 4 Received message from FRONTEND: identity: {identity}") if identity == b'cmdr': - print(f"proxy : 5 Received message from FRONTEND: discard bc of cmdr") + print("proxy : 5 Received message from FRONTEND: discard bc of cmdr") pass elif identity == b'usr': - print(f"proxy : 5 Received message from FRONTEND: sent back bc of user") + print("proxy : 5 Received message from FRONTEND: sent back bc of user") await cmd_front_cmdr_sock.send_multipart(message) else: - print(f"cmd_proxy: invalid identity") + print("cmd_proxy: invalid identity") except Exception as e: print(f"Proxy error: {e}") diff --git a/os/test/dummy_kernel_data.py b/os/test/dummy_kernel_data.py index af404b8f..6f3991e6 100644 --- a/os/test/dummy_kernel_data.py +++ b/os/test/dummy_kernel_data.py @@ -1,12 +1,12 @@ -import os -import zmq -import time -from cnc_protocol import cnc_pb2 import asyncio +import logging +import sys +import time + +import zmq import zmq.asyncio +from cnc_protocol import cnc_pb2 from util.utils import setup_socket -import logging -import sys # Configure logger logger = logging.getLogger() @@ -35,7 +35,7 @@ setup_socket(msn_sock, 'bind', 'MSN_PORT', 'Created user space mission control socket endpoint') -class k_client(): +class k_client: # Function to send a start mission command def send_start_mission(self): @@ -90,7 +90,7 @@ async def proxy(self): elif identity == b'usr': await cmd_back_sock.send_multipart(message) else: - print(f"cmd_proxy: invalid identity") + print("cmd_proxy: invalid identity") # Check for messages on the DEALER socket @@ -104,13 +104,13 @@ async def proxy(self): print(f"proxy : 4 Received message from FRONTEND: identity: {identity}") if identity == b'cmdr': - print(f"proxy : 5 Received message from FRONTEND: discard bc of cmdr") + print("proxy : 5 Received message from FRONTEND: discard bc of cmdr") pass elif identity == b'usr': - print(f"proxy : 5 Received message from FRONTEND: sent back bc of user") + print("proxy : 5 Received message from FRONTEND: sent back bc of user") await cmd_front_sock.send_multipart(message) else: - print(f"cmd_proxy: invalid identity") + print("cmd_proxy: invalid identity") except Exception as e: print(f"Proxy error: {e}") diff --git a/os/test/kernel/dummy_gabriel_client.py b/os/test/kernel/dummy_gabriel_client.py index 145759d9..4125502f 100644 --- a/os/test/kernel/dummy_gabriel_client.py +++ b/os/test/kernel/dummy_gabriel_client.py @@ -1,10 +1,12 @@ import asyncio import os import sys -from gabriel_protocol import gabriel_pb2 -from gabriel_client.websocket_client import ProducerWrapper, WebsocketClient -from cnc_protocol import cnc_pb2 + import nest_asyncio +from cnc_protocol import cnc_pb2 +from gabriel_client.websocket_client import ProducerWrapper, WebsocketClient +from gabriel_protocol import gabriel_pb2 + nest_asyncio.apply() class Dummy: @@ -25,7 +27,7 @@ async def producer(): self.heartbeats += 1 input_frame = gabriel_pb2.InputFrame() input_frame.payload_type = gabriel_pb2.PayloadType.TEXT - input_frame.payloads.append('heartbeart'.encode('utf8')) + input_frame.payloads.append(b'heartbeart') extras = cnc_pb2.Extras() # test diff --git a/os/test/kernel/dummy_telemetry_handler.py b/os/test/kernel/dummy_telemetry_handler.py index c9625e71..56fe0e93 100644 --- a/os/test/kernel/dummy_telemetry_handler.py +++ b/os/test/kernel/dummy_telemetry_handler.py @@ -1,5 +1,6 @@ import asyncio import os + import zmq from cnc_protocol import cnc_pb2 @@ -35,9 +36,9 @@ async def telemetry_handler(self): while True: try: - print(f'Telemetry Handler: Waiting for telemetry') + print('Telemetry Handler: Waiting for telemetry') msg = self.telemetry_socket.recv(flags=zmq.NOBLOCK) - print(f'Telemetry Handler: Received telemetry') + print('Telemetry Handler: Received telemetry') telemetry = cnc_pb2.Telemetry() telemetry.ParseFromString(msg) print(f'Telemetry Handler: {telemetry}') diff --git a/os/test/user/dummy_kernel_msn.py b/os/test/user/dummy_kernel_msn.py index cc63bf7c..f4182de3 100644 --- a/os/test/user/dummy_kernel_msn.py +++ b/os/test/user/dummy_kernel_msn.py @@ -1,8 +1,9 @@ import asyncio import logging import sys -from cnc_protocol import cnc_pb2 + import zmq +from cnc_protocol import cnc_pb2 from util.utils import SocketOperation, setup_socket # Configure logger diff --git a/os/user/common/MissionController.py b/os/user/common/MissionController.py index 9ef0891e..4e7e68cc 100644 --- a/os/user/common/MissionController.py +++ b/os/user/common/MissionController.py @@ -1,25 +1,25 @@ +import asyncio import importlib +import logging import os -import subprocess import shutil +import subprocess import sys from zipfile import ZipFile + import requests import zmq -import asyncio -import logging - -from util.utils import SocketOperation, setup_socket -from system_call_stubs.DroneStub import DroneStub -from system_call_stubs.ComputeStub import ComputeStub from cnc_protocol import cnc_pb2 +from system_call_stubs.ComputeStub import ComputeStub +from system_call_stubs.DroneStub import DroneStub +from util.utils import SocketOperation, setup_socket logger = logging.getLogger(__name__) -class MissionController(): +class MissionController: def __init__(self, user_path): self.isTerminated = False self.tm = None @@ -105,22 +105,22 @@ def reload_mission(self): def start_mission(self): if self.tm: - logger.info(f"mission already running") + logger.info("mission already running") return else: # first time mission, create a task manager - import common.TaskManager as tm + import common.TaskManager as tm - logger.info(f"start the mission") + logger.info("start the mission") if self.reload : self.reload_mission() - import project.Mission as msn # import the mission module instead of attribute of the module for the reload to work + import project.Mission as msn # import the mission module instead of attribute of the module for the reload to work self.reload = True msn.Mission.define_mission(self.transitMap, self.task_arg_map) # start the tm - logger.info(f"start the task manager") + logger.info("start the task manager") self.tm = tm.TaskManager(self.drone, self.compute, self.transitMap, self.task_arg_map) self.tm_coroutine = asyncio.create_task(self.tm.run()) diff --git a/os/user/common/TaskManager.py b/os/user/common/TaskManager.py index 23af6d2c..bc9cf9ab 100644 --- a/os/user/common/TaskManager.py +++ b/os/user/common/TaskManager.py @@ -1,16 +1,17 @@ import asyncio -import interface.Task as taskitf -import project.task_defs.TrackTask as track -import project.task_defs.DetectTask as detect +import logging +import queue + +import interface.Task as taskitf import project.task_defs.AvoidTask as avoid +import project.task_defs.DetectTask as detect import project.task_defs.TestTask as test -import queue -import logging +import project.task_defs.TrackTask as track logger = logging.getLogger(__name__) -class TaskManager(): +class TaskManager: def __init__(self, drone, compute, transit_map, task_arg_map): super().__init__() @@ -51,7 +52,7 @@ async def init_task(self): self.start_task_id = self.retrieve_next_task("start", None) logger.info('create start task') start_task = self.create_task(self.start_task_id) - if start_task != None: + if start_task is not None: # set the current task self.curr_task_id = start_task.task_id logger.info(f"start task, current taskid:{self.curr_task_id}\n") @@ -66,7 +67,7 @@ async def init_task(self): def create_task(self, task_id): logger.info(f'taskid{task_id}') - if (task_id in self.task_arg_map.keys()): + if task_id in self.task_arg_map: if (self.task_arg_map[task_id].task_type == taskitf.TaskType.Detect): logger.info('Detect task') return detect.DetectTask(self.drone, self.compute, task_id, self.trigger_event_queue, self.task_arg_map[task_id]) @@ -82,15 +83,15 @@ def create_task(self, task_id): return None def stop_task(self): - logger.info(f'Stopping current task!') + logger.info('Stopping current task!') if self.taskCurrentCoroutinue: # stop all the transitions of the task self.currentTask.stop_trans() - logger.info(f'transitions in the current task stopped!') + logger.info('transitions in the current task stopped!') is_canceled = self.taskCurrentCoroutinue.cancel() if is_canceled: - logger.info(f' task cancelled successfully') + logger.info(' task cancelled successfully') def start_task(self, task): logger.info(f'start the task! task: {str(task)}') @@ -120,7 +121,7 @@ async def run(self): item = self.trigger_event_queue.get() task_id = item[0] trigger_event = item[1] - logger.info(f"Trigger one event! \n") + logger.info("Trigger one event! \n") logger.info(f"Task id {task_id} \n") logger.info(f"event {trigger_event} \n") if (task_id == self.get_current_task()): diff --git a/os/user/common/__main__.py b/os/user/common/__main__.py index 61ba9a3a..3298934e 100644 --- a/os/user/common/__main__.py +++ b/os/user/common/__main__.py @@ -1,8 +1,9 @@ -import asyncio +import asyncio import logging +import os import sys + from MissionController import MissionController -import os logger = logging.getLogger() logger.setLevel(logging.INFO) diff --git a/os/user/interface/Task.py b/os/user/interface/Task.py index b4709c7d..bf5b77c6 100644 --- a/os/user/interface/Task.py +++ b/os/user/interface/Task.py @@ -2,12 +2,12 @@ # # SPDX-License-Identifier: GPL-2.0-only -from abc import ABC, abstractmethod import functools import logging import threading +from abc import ABC, abstractmethod + from aenum import Enum -import inspect logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -18,7 +18,7 @@ class TaskType(Enum): Avoid = 3 Test = 4 -class TaskArguments(): +class TaskArguments: def __init__(self, task_type, transitions_attributes, task_attributes): self.task_type = task_type self.task_attributes = task_attributes @@ -46,17 +46,17 @@ def get_task_id(self): def _exit(self): # kill all the transitions - logger.info(f"**************exit the task**************\n") + logger.info("**************exit the task**************\n") self.stop_trans() self.trigger_event_queue.put((self.task_id, "done")) def stop_trans(self): - logger.info(f"**************stopping the transitions**************\n") + logger.info("**************stopping the transitions**************\n") for trans in self.trans_active: if trans.is_alive(): trans.stop() trans.join() - logger.info(f"**************the transitions stopped**************\n") + logger.info("**************the transitions stopped**************\n") @classmethod @@ -74,8 +74,10 @@ async def wrapper(self, *args, **kwargs): return wrapper + @abstractmethod def pause(self): pass - + + @abstractmethod def resume(self): pass diff --git a/os/user/interface/Transition.py b/os/user/interface/Transition.py index a7978905..26fddd3b 100644 --- a/os/user/interface/Transition.py +++ b/os/user/interface/Transition.py @@ -1,7 +1,6 @@ -from abc import ABC, abstractmethod import logging import threading - +from abc import ABC, abstractmethod logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/os/user/system_call_stubs/ComputeStub.py b/os/user/system_call_stubs/ComputeStub.py index bb7052b8..422e059b 100644 --- a/os/user/system_call_stubs/ComputeStub.py +++ b/os/user/system_call_stubs/ComputeStub.py @@ -5,11 +5,10 @@ import asyncio import logging import os + import zmq from cnc_protocol import cnc_pb2 -from util.utils import setup_socket -from util.utils import SocketOperation -from enum import Enum +from util.utils import SocketOperation, setup_socket logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -39,7 +38,7 @@ async def wait(self): def set (self): self.event.set() -class ComputeStub(): +class ComputeStub: def __init__(self): self.seqNum = 1 # set the initial seqNum to 1 caz cnc proto does not support to show 0 self.seqNum_res = {} diff --git a/os/user/system_call_stubs/DroneStub.py b/os/user/system_call_stubs/DroneStub.py index 38d2be48..65375298 100644 --- a/os/user/system_call_stubs/DroneStub.py +++ b/os/user/system_call_stubs/DroneStub.py @@ -1,11 +1,11 @@ import asyncio import logging import os +from enum import Enum + import zmq from cnc_protocol import cnc_pb2 -from enum import Enum -from util.utils import setup_socket -from util.utils import SocketOperation +from util.utils import SocketOperation, setup_socket logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/os/util/timer.py b/os/util/timer.py index 3dde071f..3c421181 100644 --- a/os/util/timer.py +++ b/os/util/timer.py @@ -1,7 +1,7 @@ +import logging import time from dataclasses import dataclass, field -from typing import Optional -import logging + class TimerError(Exception): """A custom exception used to report errors in use of Timer class""" @@ -9,11 +9,11 @@ class TimerError(Exception): @dataclass class Timer: logger: logging.Logger - name: Optional[str] = None + name: str | None = None text: str = "{} took {:0.4f} seconds" - max_frequency: Optional[int] = None - _start_time: Optional[float] = field(default=None, init=False, repr=False) - _last_log_time: Optional[int] = None + max_frequency: int | None = None + _start_time: float | None = field(default=None, init=False, repr=False) + _last_log_time: int | None = None def __enter__(self): self.start() @@ -25,14 +25,14 @@ def __exit__(self, exc_type, exc_value, traceback): def start(self) -> None: """Start a new timer""" if self._start_time is not None: - raise TimerError(f"Timer is running. Use .stop() to stop it") + raise TimerError("Timer is running. Use .stop() to stop it") self._start_time = time.perf_counter() def stop(self) -> float: """Stop the timer, and report the elapsed time""" if self._start_time is None: - raise TimerError(f"Timer is not running. Use .start() to start it") + raise TimerError("Timer is not running. Use .start() to start it") # Calculate elapsed time elapsed_time = time.perf_counter() - self._start_time diff --git a/os/util/utils.py b/os/util/utils.py index ea2b530d..bf9eb0a5 100644 --- a/os/util/utils.py +++ b/os/util/utils.py @@ -1,6 +1,7 @@ -from enum import Enum import logging import os +from enum import Enum + import zmq import zmq.asyncio @@ -41,13 +42,13 @@ async def lazy_pirate_request(socket, payload, ctx, server_endpoint, retries=3, socket.send(payload) retries_left = retries - while retries_left == None or retries_left > 0: + while retries_left is None or retries_left > 0: # Check if reply received within timeout poll_result = await socket.poll(timeout) if (poll_result & zmq.POLLIN) != 0: reply = await socket.recv() return (socket, reply) - if retries_left != None: + if retries_left is not None: retries_left -= 1 logger.warning(f"Request timeout for {server_endpoint=}")