From f4e965774ca80545498e074c149f9deca88dd615 Mon Sep 17 00:00:00 2001 From: Hansen Date: Tue, 4 Jun 2024 18:34:38 -0700 Subject: [PATCH 01/10] switch timestamp to milliseconds since epoch --- src/client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/client.py b/src/client.py index 5c614f0..c09432d 100644 --- a/src/client.py +++ b/src/client.py @@ -106,7 +106,7 @@ def interpret_packedmission(recvd): # Send telemetry to server telemetry = b"TL" - telemetry += struct.pack('2i12f', int(time.time()), int(cs.wpno), + telemetry += struct.pack('2i12f', int(time.time() * 1000), int(cs.wpno), cs.lat, cs.lng, cs.alt, cs.roll, cs.pitch, cs.yaw, cs.airspeed, cs.groundspeed, cs.verticalspeed, From 910ee3b9336fb97f11334bb2a8e5a33ace0419f6 Mon Sep 17 00:00:00 2001 From: Hansen Date: Wed, 5 Jun 2024 19:28:07 -0700 Subject: [PATCH 02/10] Handling more waypoint types Co-authored-by: Ram Jayakumar --- api_spec.yml | 2 +- src/server/gcomhandler.py | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/api_spec.yml b/api_spec.yml index 21ccddd..9d217ba 100644 --- a/api_spec.yml +++ b/api_spec.yml @@ -372,7 +372,7 @@ components: format: float64 command: type: string - enum: ['WAYPOINT', 'LOITER_UNLIM', 'DO_VTOL_TRANSITION', 'DO_CHANGE_SPEED'] + description: "Put the waypoint command here as in MissionPlanner, i.e., `LOITER_UNLIM` or `DO_CHANGE_SPEED`. Default value: `WAYPOINT`" param1: type: number format: integer diff --git a/src/server/gcomhandler.py b/src/server/gcomhandler.py index 58b6f27..500169a 100644 --- a/src/server/gcomhandler.py +++ b/src/server/gcomhandler.py @@ -9,6 +9,7 @@ from server.common.wpqueue import WaypointQueue, Waypoint from server.common.status import Status from server.common.sharedobject import SharedObject +from server.common.encoders import command_string_to_int, command_int_to_string def plot_shape(points, color, close=False, scatter=True): adjust = 0 if close else 1 @@ -116,8 +117,8 @@ def post_queue(): altitude = last_altitude command = wpdict.get('command', "WAYPOINT") - if command not in ["WAYPOINT", "LOITER_UNLIM", "DO_VTOL_TRANSITION", "DO_CHANGE_SPEED"]: - command = "WAYPOINT" + # converts any unknown waypoint types to WAYPOINT + command = command_int_to_string(command_string_to_int(command)) param1 = wpdict.get('param1', 0) param2 = wpdict.get('param2', 0) From c07050c64ca154515c2dc81f9ff459ed46955d3d Mon Sep 17 00:00:00 2001 From: Hansen Date: Wed, 5 Jun 2024 19:28:55 -0700 Subject: [PATCH 03/10] support time in milliseconds --- src/client.py | 2 +- src/server/common/status.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/client.py b/src/client.py index c09432d..7af8979 100644 --- a/src/client.py +++ b/src/client.py @@ -106,7 +106,7 @@ def interpret_packedmission(recvd): # Send telemetry to server telemetry = b"TL" - telemetry += struct.pack('2i12f', int(time.time() * 1000), int(cs.wpno), + telemetry += struct.pack('Qi12f', int(time.time() * 1000), int(cs.wpno), cs.lat, cs.lng, cs.alt, cs.roll, cs.pitch, cs.yaw, cs.airspeed, cs.groundspeed, cs.verticalspeed, diff --git a/src/server/common/status.py b/src/server/common/status.py index 412c8ce..1352e82 100644 --- a/src/server/common/status.py +++ b/src/server/common/status.py @@ -29,7 +29,7 @@ def __init__(self, timestamp = 0, waypoint_number = -1, self._wvl: float = wind_velocity def encoded_status(self) -> bytes: - return struct.pack('2i12f', + return struct.pack('Qi12f', self._timestamp, self._wpn, self._lat, @@ -51,7 +51,7 @@ def decode_status(self, status_bytes: bytes) -> None: self._rol, self._pch, self._yaw, self._asp, self._gsp, self._vsp, self._btv, - self._wdr, self._wvl) = struct.unpack('2i12f', status_bytes) + self._wdr, self._wvl) = struct.unpack('Qi12f', status_bytes) def as_dictionary(self) -> dict: return { From 499141a9e9dbba60ab25ac0031a06f994b2f9876 Mon Sep 17 00:00:00 2001 From: Hansen Date: Wed, 5 Jun 2024 19:29:34 -0700 Subject: [PATCH 04/10] add endpoint for insertion --- src/client.py | 64 ++++++++++++++++++++++++++----- src/server/common/sharedobject.py | 49 ++++++++++++++++++++--- src/server/gcomhandler.py | 56 +++++++++++++-------------- src/server/mps_server.py | 13 +++++++ 4 files changed, 137 insertions(+), 45 deletions(-) diff --git a/src/client.py b/src/client.py index 7af8979..d9c0c9a 100644 --- a/src/client.py +++ b/src/client.py @@ -34,9 +34,8 @@ print("Sockets Created") wp_array = [] -upcoming_mission = False -fence_exclusive = False -fence_type = "" +upcoming_mission = "" +insert_index = -1 def get_altitude_standard(standard): if standard == "AGL": @@ -90,7 +89,9 @@ def interpret_normal(recvd): return msg.split() def interpret_packedmission(recvd): - ret = ["NEXT"] + global upcoming_mission + ret = [upcoming_mission] + upcoming_mission = "" #print(recvd) sizeof_waypoint = struct.calcsize('3f5h') @@ -128,7 +129,7 @@ def interpret_packedmission(recvd): time.sleep(10) continue - if (upcoming_mission): + if (upcoming_mission != ""): argv = interpret_packedmission(recvd) else: argv = interpret_normal(recvd) @@ -140,14 +141,12 @@ def interpret_packedmission(recvd): break else: if cmd == "NEW_MISSION": - #Enter guided and await new mission waypoints + #Await new mission waypoints wp_array = [] - upcoming_mission = True + upcoming_mission = "NEXT" print("NEW_MISSION - About to recieve new mission") elif cmd == "NEXT": - upcoming_mission = False - print(cmd, argv) #set mission @@ -159,9 +158,54 @@ def interpret_packedmission(recvd): print("NEXT - new mission set") + elif cmd == "NEW_INSERT": + #Await new mission waypoints + wp_array = [] + upcoming_mission = "INSERT" + insert_index = int(argv[0]) + print("NEW_INSERT - About to recieve waypoints for insertion") + + elif cmd == "INSERT": + print(cmd, argv) + + #grab old mission + old_mission = [] + + current_wp = int(cs.wpno) + numwp = MAV.getWPCount() + + for i in range(0, numwp): + try: + old_mission.append(MAV.getWP(MAV.sysidcurrent, MAV.compidcurrent, i)) + except: + print("WARNING - waypoint get failed for waypoint number", i) + + #set new mission + new_mission = [] + + #inserts old mission wps that are before the index + #convert each Locationwp instance in old_mission into a tuple for upload_mission + new_mission.extend((wp.lat, wp.lng, wp.alt, wp.id, int(wp.p1), int(wp.p2), int(wp.p3), int(wp.p4)) + for wp in old_mission[current_wp : current_wp + insert_index]) + + #inserts new mission wps + new_mission.extend(argv) + + #inserts old mission wps that are after the index + new_mission.extend((wp.lat, wp.lng, wp.alt, wp.id, int(wp.p1), int(wp.p2), int(wp.p3), int(wp.p4)) + for wp in old_mission[current_wp + insert_index :]) + + upload_mission(new_mission) + + # Cycles mode so drone responds to new mission + Script.ChangeMode("Loiter") + Script.ChangeMode("Auto") + + print("INSERT - new waypoints inserted") + elif cmd == "PUSH": #TODO: currently nonfunctional - must refactor - see #75 on github - wptotal = MAV.getWPCount() + wptotal = MAV.geCount() MAV.setWPTotal(wptotal + 1) # Upload waypoints diff --git a/src/server/common/sharedobject.py b/src/server/common/sharedobject.py index 36d9a5e..1f7ee31 100644 --- a/src/server/common/sharedobject.py +++ b/src/server/common/sharedobject.py @@ -1,7 +1,9 @@ #from multiprocessing import Lock from threading import Lock +from typing import Optional from server.common.status import Status +from server.common.wpqueue import WaypointQueue class SharedObject(): def __init__(self): @@ -13,11 +15,17 @@ def __init__(self): self._currentmission_lk = Lock() # New mission fields - self._newmission = [] + self._newmission: WaypointQueue = [] self._newmission_lk = Lock() - self._newmission_flag = False + self._newmission_flag: bool = False self._newmission_flag_lk = Lock() + # New insertion fields + self._newinsert: WaypointQueue = [] + self._newinsert_lk = Lock() + self._newinsert_flag: bool = False + self._newinsert_flag_lk = Lock() + # Status fields self._status: Status = Status() self._status_lk = Lock() @@ -142,10 +150,10 @@ def mps_currentmission_update(self, num): self._currentmission_lk.release() # newmission methods - def gcom_newmission_flagcheck(self): + def gcom_newmission_flagcheck(self) -> bool: return self._newmission_flag - def gcom_newmission_set(self, wpq): + def gcom_newmission_set(self, wpq: WaypointQueue) -> bool: self._newmission_flag_lk.acquire() self._newmission_flag = True @@ -157,7 +165,7 @@ def gcom_newmission_set(self, wpq): return True - def mps_newmission_get(self): + def mps_newmission_get(self) -> Optional[WaypointQueue]: if self._newmission_flag: self._newmission_flag_lk.acquire() self._newmission_flag = False @@ -178,6 +186,37 @@ def mps_newmission_get(self): else: return None + def gcom_newinsert_flagcheck(self) -> bool: + return self._newinsert_flag + + def gcom_newinsert_set(self, wpq: WaypointQueue) -> bool: + self._newinsert_flag_lk.acquire() + self._newinsert_flag = True + + self._newinsert_lk.acquire() + self._newinsert = wpq + + self._newinsert_lk.release() + self._newinsert_flag_lk.release() + + return True + + def mps_newinsert_get(self) -> Optional[WaypointQueue]: + if self._newinsert_flag: + self._newinsert_flag_lk.acquire() + self._newinsert_flag = False + + self._newinsert_lk.acquire() + ret = self._newinsert + self._newinsert = [] + + self._newinsert_lk.release() + self._newinsert_flag_lk.release() + + return ret + else: + return None + # Status methods def set_status(self, updated: Status) -> None: self._status_lk.acquire() diff --git a/src/server/gcomhandler.py b/src/server/gcomhandler.py index 500169a..cca43bf 100644 --- a/src/server/gcomhandler.py +++ b/src/server/gcomhandler.py @@ -135,43 +135,39 @@ def post_queue(): return "ok", 200 - @app.route("/prepend", methods=['POST']) + @app.route("/insert", methods=['POST']) def insert_wp(): payload = request.get_json() - if not('latitude' in payload) or not('longitude' in payload): - return "Latitude and Longitude cannot be null", 400 - - self._so.gcom_currentmission_trigger_update() - while self._so._currentmission_flg_ready == False: - pass - ret = self._so.gcom_currentmission_get() - - wpno = int(self._so.get_status()._wpn) - remaining = ret[wpno-1:] - wp = Waypoint(0, payload['name'], payload['latitude'], payload['longitude'], payload['altitude']) + ret: Status = self._so.get_status() + last_altitude = ret._alt if ret != () else 50 - if payload['altitude'] is not None: - wp = Waypoint(0, payload['name'], payload['latitude'], payload['longitude'], remaining[-1]._alt) + # gets new waypoints + new_waypoints = [] + for wpdict in payload: + altitude = wpdict.get('altitude') + if altitude != None: + last_altitude = altitude + else: + altitude = last_altitude - remaining.insert(1, wp) - self._so.gcom_newmission_set(WaypointQueue(remaining.copy())) + command = wpdict.get('command', "WAYPOINT") + # converts any unknown waypoint types to WAYPOINT + command = command_int_to_string(command_string_to_int(command)) - return "ok", 200 + param1 = wpdict.get('param1', 0) + param2 = wpdict.get('param2', 0) + param3 = wpdict.get('param3', 0) + param4 = wpdict.get('param4', 0) + + wp = Waypoint(wpdict['id'], wpdict['name'], wpdict['latitude'], wpdict['longitude'], last_altitude, + command, param1, param2, param3, param4) + new_waypoints.append(wp) - - @app.route("/append", methods=['POST']) - def append_wp(): - payload = request.get_json() - - if not('latitude' in payload) or not('longitude' in payload): - return "Latitude and Longitude cannot be null", 400 - - ret: Status = self._so.get_status() - last_altitude = ret._alt if ret != () else 50 - - wp = Waypoint(0, payload['name'], payload['latitude'], payload['longitude'], last_altitude) - self._so.append_wp_set(wp) + # insert new waypoints start at index + self._so.gcom_newinsert_set(WaypointQueue(new_waypoints.copy())) + copy = WaypointQueue(new_waypoints.copy()).aslist() + new_waypoints.clear() return "ok", 200 diff --git a/src/server/mps_server.py b/src/server/mps_server.py index a584679..3a89316 100644 --- a/src/server/mps_server.py +++ b/src/server/mps_server.py @@ -171,6 +171,19 @@ def check_shared_object(self, current_wpn): if push_wp: self.server._instructions.push(f"PUSH {str(push_wp)}") + # Check for a new insertion + insertwpq = self.server._so.mps_newinsert_get() + if insertwpq != None: + # Place instruction for new insertion onto the queue + self.server._instructions.push(f"NEW_INSERT {0}") + + # Prepare packed mission + missionbytes = b"" + while (not insertwpq.empty()): + curr: Waypoint = insertwpq.pop() + missionbytes += waypoint_encode(curr) + self.server._instructions.push(missionbytes) + # Check for a new mission nextwpq = self.server._so.mps_newmission_get() if nextwpq != None: From 00dfbfa7e667e6f4d557a45d2905787922d60d43 Mon Sep 17 00:00:00 2001 From: Hansen Date: Wed, 5 Jun 2024 20:05:51 -0700 Subject: [PATCH 05/10] doc updates --- README.md | 227 +++------------------------------------- api_spec.yml | 21 ++-- postman_collection.json | 47 ++++----- 3 files changed, 45 insertions(+), 250 deletions(-) diff --git a/README.md b/README.md index 1f5deef..cc99b6b 100644 --- a/README.md +++ b/README.md @@ -6,9 +6,9 @@ 2. [Endpoints](#endpoints) 3. [Sockets](#sockets) -# Instructions +## Instructions -## SITL & MissionPlanner +### SITL & MissionPlanner 1. In order to run SITL on your local machine, you will need to have Docker installed. For installation instructions, refer to the following: @@ -48,7 +48,7 @@ following: 7. If you have completed all of the above steps you should be ready to use SITL with MissionPlanner. If you see a drone show up on the map then you should be ready to go. -## Using MissionPlanner-Scripts +### Using MissionPlanner-Scripts > [!NOTE] > MissionPlanner currently only works on Windows @@ -92,220 +92,27 @@ Then, enter the src directory and run the `pytest` command via Poetry: poetry run pytest ``` -# Endpoints +## Endpoints -## (GET) /queue +See `api_spec.yml` or `postman_collection.json` for up-to-date information on endpoints. -Returns the remaining list of waypoints in the order provided by the queue. GCOM stores longitudes and latitudes internally, so we really only need the order of names of waypoints. +## Sockets -Waypoints that have been passed and removed from the queue, obviously, should not be displayed here either. +The status WebSocket client connects to `localhost:1323` by default. The hostname and port can be changed via command-line arguments. -Altitude is measured relative to sea level. - -Example response body: - -```json -[ - { - "id": 1, - "name": "Alpha", - "longitude": 38.83731, - "latitude": -20.48321, - "altitude": 50.7 - }, - { - "id": 2, - "name": "Beta", - "longitude": 38.83731, - "latitude": -20.48321, - "altitude": 50.7 - } -] -``` - -## (POST) /queue - -POST request containing a list of waypoints with names and longitude, latitude, and altitude values. If altitude is nil, carry on with the same altitude as you had last waypoint. - -Previous queue should be overwritten if there is already one in place. - -Longitude, name, and latitude must not be null/empty. Returns a Bad Request status code and error message in that case. -Longitude and latitude in degrees, altitude in meters. - -Altitude is measured relative to sea level. - -Return status code 200 if successfully POSTed. - -Example request body: - -```json -[ - { - "id": 1, - "name": "Alpha", - "longitude": 38.83731, - "latitude": -20.48321, - "altitude": 50.7 - }, - { - "id": 2, - "name": "Beta", - "longitude": 38.83731, - "latitude": -20.48321, - "altitude": null - } -] -``` - -## (GET) /clear - -Call this endpoint to clear the current contents of the queue. The drone will no longer pursue the mission, as all waypoints have been removed. - -An alternative to this endpoint is to call `POST /queue` with an empty queue as the body, since it overwrites the mission. - -## (GET) /status - -GET request returns the aircraft status. -Velocity in m/s. Altitude in meters and is relative to sea level. Longitude, latitude, heading in degrees. - -Example response: - -```json -{ - "velocity": 22.2, - "longitude": 38.3182, - "latitude": 82.111, - "altitude": 28.1111, - "heading": 11.2, - "batteryvoltage": 1.5 -} -``` - -## (GET) /lock - -Stops the aircraft from moving based on the Mission Planner scripts' waypoint queue loading functionality, maintaining the queue internally. -Return Bad Request if the aircraft is already locked, -or the queue is empty. - -It is still be possible to run (POST) /queue while the aircraft is locked. - -This won't literally lock the aircraft either, i.e. -we can still manually set waypoints with Mission Planner. This just pauses the loading functionality of the queue program. If currently moving toward a waypoint, stop moving toward it by removing it. - -## (GET) /unlock - -Resume moving the aircraft based on the currently stored queue. Returns a Bad Request status code and an error message if the aircraft is already unlocked. - -## (POST) /takeoff - -POST request containing an altitude that is measured relative to sea level. - -The altitude cannot be null. Returns a Bad Request status code and error message in that case. Altitude is in meters. Return status code 200 if successfully POSTed. - -Example request body: - -```json -{ - "altitude": 50.7 -} -``` - -## (POST) /rtl - -Aircraft returns to home waypoint and lands (return-to-launch). Returns a Bad Request status code and error message if the drone could not execute the operation. - -## (GET) /land - -Aircraft stops at its current position and lands. Enters loiter mode before landing. Returns a Bad Request status code and error message if the drone could not execute the operation. - -## (POST) /home - -POST request containing a waypoint whose longitude, latitiude and altitiude will be the basis for the new home waypoint. All other fields will be ignored. - -Longitude, latitude and altitude must not be null/empty. Returns a Bad Request status code and error message in that case. - -Longitude and latitude in degrees. -Altitude in meters and is relative to sea level. -Return status code 200 if successfully POSTed. - -Example request body: +Every 100ms, it will emit the `drone_update` event with the following information: ```json { - "id": 1, - "name": "Alpha", - "longitude": 38.83731, - "latitude": -20.48321, - "altitude": 50.7 + "timestamp": 0, + "latitude": 0.0, + "longitude": 0.0, + "altitude": 0.0, + "vertical_velocity": 0.0, + "velocity": 0.0, + "heading": 0.0, + "battery_voltage": 0.0 } ``` -## (PUT) /flightmode - -This PUT request allows you to modify various settings for a drone: - -- **Flight Mode:** Change the active flight mode by setting the `flight_mode` key to one of the following: `loiter`, `stabilize`, `auto`, or `guided`. -- **Drone Type:** Modify the drone configuration by setting the `drone_type` key to either `vtol` or `plane`. -- **Altitude Reference:** Update the altitude measurement standard by setting the `altitude_standard` key to `AGL` (Above Ground Level) or `ASL` (Above Sea Level). - -Each of these key-value pairs is optional; you can include any, all, or none of them in the JSON request body. - -### Example Request Body - -```json -{ - "flight_mode": "loiter", - "drone_type": "vtol", - "altitude_standard": "ASL" -} -``` - -## (POST) /insert -Inserts a new waypoint at the beginning of the queue. Drone should immediately head to this waypoint when the request is sent. - -Example request body: - -```json -{ - "id": 2, - "name": "Beta", - "longitude": 18.43731, - "latitude": -19.24251, - "altitude": 42.7 -} -``` - -# Sockets - -A connection can be established through a Socket endpoint (set through command line argument, port 9001 by default). An example Node.js client has been provided in `testing/socket.js` that establishes a connection, and continually sends/recieves status information every 500ms. - -## Events (server-side) - -The server is listening for the following events: - -### connect - -Socket client connects. Outputs to console to confirm connection. - -### disconnect - -Socket client disconnects. Outputs to console to confirm disconnection. - -### message - -On recieving a `message` event, the server emits another `message` event in response, carrying a JSON containing basic drone status information. JSON Response template: - -```json -{ - "velocity": 22.2, - "longitude": 38.3182, - "latitude": 82.111, - "altitude": 28.1111, - "heading": 11.2, - "batteryvoltage": 1.5 -} -``` - -## Heartbeat - -We are using the Flask SocketIO library for our implementation. By default, the server pings the client every 25 seconds. +The timestamp is the number of milliseconds since the epoch. diff --git a/api_spec.yml b/api_spec.yml index 9d217ba..cd6916d 100644 --- a/api_spec.yml +++ b/api_spec.yml @@ -67,21 +67,18 @@ paths: description: successful operation "400": description: bad request - /prepend: + /insert: post: tags: - queue - summary: Insert a waypoint onto the front of the queue. - responses: - "200": - description: successful operation - "400": - description: bad request - /append: - post: - tags: - - queue - summary: Insert a waypoint onto the back of the queue. + summary: Inserts waypoints immediately before the current waypoint + requestBody: + content: + application/json: + schema: + type: array + items: + $ref: "#/components/schemas/Waypoint" responses: "200": description: successful operation diff --git a/postman_collection.json b/postman_collection.json index 58421cd..2f5c67f 100644 --- a/postman_collection.json +++ b/postman_collection.json @@ -1,9 +1,9 @@ { "info": { - "_postman_id": "ca2d086f-3956-40cf-ae15-4403aa2f65fd", + "_postman_id": "03a8d1c7-9e89-471a-8146-9fd532858571", "name": "MissionPlannerScripts", "schema": "https://schema.getpostman.com/json/collection/v2.1.0/collection.json", - "_exporter_id": "32319969" + "_exporter_id": "30667343" }, "item": [ { @@ -24,7 +24,7 @@ ], "body": { "mode": "raw", - "raw": "[\n {\n \"id\": 0,\n \"name\": \"string\",\n \"longitude\": 0,\n \"latitude\": 0,\n \"altitude\": 0\n }\n]", + "raw": "", "options": { "raw": { "language": "json" @@ -56,7 +56,7 @@ ], "body": { "mode": "raw", - "raw": "[\n {\n \"id\": 0,\n \"name\": \"Alpha\",\n \"latitude\": 38.3138848,\n \"longitude\": -76.5499264,\n \"altitude\": 100\n },\n {\n \"id\": 1,\n \"name\": \"Alpha\",\n \"latitude\": 38.3180233,\n \"longitude\": -76.5576053,\n \"altitude\": 100\n },\n {\n \"id\": 2,\n \"name\": \"Alpha\",\n \"latitude\": 38.3200772,\n \"longitude\": -76.5527773,\n \"altitude\": 100\n },\n {\n \"id\": 3,\n \"name\": \"Alpha\",\n \"latitude\": 38.3195385,\n \"longitude\": -76.5394735,\n \"altitude\": 100\n },\n {\n \"id\": 4,\n \"name\": \"Alpha\",\n \"latitude\": 38.3112889,\n \"longitude\": -76.5240669,\n \"altitude\": 100\n },\n {\n \"id\": 5,\n \"name\": \"Alpha\",\n \"latitude\": 38.3035098,\n \"longitude\": -76.5376282,\n \"altitude\": 100\n },\n {\n \"id\": 6,\n \"name\": \"Alpha\",\n \"latitude\": 38.3012197,\n \"longitude\": -76.5467262,\n \"altitude\": 100\n },\n {\n \"id\": 7,\n \"name\": \"Alpha\",\n \"latitude\": 38.3050253,\n \"longitude\": -76.5667677,\n \"altitude\": 100\n },\n {\n \"id\": 8,\n \"name\": \"Alpha\",\n \"latitude\": 38.2929007,\n \"longitude\": -76.5730762,\n \"altitude\": 100\n },\n {\n \"id\": 9,\n \"name\": \"Alpha\",\n \"latitude\": 38.2917892,\n \"longitude\": -76.5388727,\n \"altitude\": 100\n },\n {\n \"id\": 10,\n \"name\": \"Alpha\",\n \"latitude\": 38.3024995,\n \"longitude\": -76.5376711,\n \"altitude\": 100\n }\n]" + "raw": "[\n {\n \"id\": 0,\n \"name\": \"Alpha\",\n \"latitude\": 38.3143531,\n \"longitude\": -76.5594292,\n \"altitude\": 100\n },\n {\n \"id\": 6,\n \"name\": \"Alpha\",\n \"latitude\": 38.3012197,\n \"longitude\": -76.5467262,\n \"altitude\": 100,\n \"command\":\"DO_CHANGE_SPEED\",\n \"param1\":1,\n \"param2\":10\n },\n {\n \"id\": 1,\n \"name\": \"Alpha\",\n \"latitude\": 38.3180233,\n \"longitude\": -76.5576053,\n \"altitude\": 100,\n \"command\":\"WAYPOINT\",\n \"param1\":3,\n \"param2\":4,\n \"param3\":5,\n \"param4\":6\n },\n {\n \"id\": 5,\n \"name\": \"Alpha\",\n \"latitude\": 38.3035098,\n \"longitude\": -76.5376282,\n \"altitude\": 100,\n \"command\":\"DO_VTOL_TRANSITION\",\n \"param1\":3,\n \"param4\":4\n },\n {\n \"id\": 2,\n \"name\": \"Alpha\",\n \"latitude\": 38.3200772,\n \"longitude\": -76.5527773,\n \"altitude\": 100,\n \"command\":\"LOITER_UNLIM\",\n \"param3\":0\n },\n {\n \"id\": 3,\n \"name\": \"Alpha\",\n \"latitude\": 38.3195385,\n \"longitude\": -76.5394735,\n \"altitude\": 100\n },\n {\n \"id\": 4,\n \"name\": \"Alpha\",\n \"latitude\": 38.3112889,\n \"longitude\": -76.5240669,\n \"altitude\": 100,\n \"param3\":-1\n }\n \n]" }, "url": { "raw": "{{ base_url }}/queue", @@ -72,13 +72,13 @@ "response": [] }, { - "name": "prepend", + "name": "insert", "request": { "method": "POST", "header": [], "body": { "mode": "raw", - "raw": " {\r\n \"id\": 0,\r\n \"name\": \"Inserted\",\r\n \"latitude\": 38.3138848,\r\n \"longitude\": -76.5499264,\r\n \"altitude\": 100\r\n }", + "raw": "[{\r\n \"id\": 0,\r\n \"name\": \"Inserted\",\r\n \"latitude\": 38.3096384,\r\n \"longitude\": -76.5514048,\r\n \"altitude\": 100\r\n}]", "options": { "raw": { "language": "json" @@ -86,33 +86,15 @@ } }, "url": { - "raw": "{{ base_url }}/prepend", + "raw": "{{ base_url }}/insert", "host": [ "{{ base_url }}" ], "path": [ - "prepend" + "insert" ] }, - "description": "Insert waypoint at beginning of queue." - }, - "response": [] - }, - { - "name": "append", - "request": { - "method": "POST", - "header": [], - "url": { - "raw": "{{ base_url }}/append", - "host": [ - "{{ base_url }}" - ], - "path": [ - "append" - ] - }, - "description": "Insert a waypoint onto the back of the queue." + "description": "Insert a waypoint onto the front of the queue." }, "response": [] }, @@ -227,7 +209,7 @@ ], "body": { "mode": "raw", - "raw": "{\n \"altitude\": 100\n}" + "raw": "{\n \"altitude\": 25\n}" }, "url": { "raw": "{{ base_url }}/takeoff", @@ -343,6 +325,15 @@ "request": { "method": "POST", "header": [], + "body": { + "mode": "raw", + "raw": "{\r\n\t\t\"id\": 0,\r\n\t\t\"name\": \"string\",\r\n\t\t\"latitude\": 38.3171058,\r\n\t\t\"longitude\": -76.5517151,\r\n\t\t\"altitude\": 100\r\n}", + "options": { + "raw": { + "language": "json" + } + } + }, "url": { "raw": "{{ base_url }}/land", "host": [ From d44cb036082536939579056a3f6c9e88683083b7 Mon Sep 17 00:00:00 2001 From: Hansen Date: Wed, 5 Jun 2024 20:07:01 -0700 Subject: [PATCH 06/10] explicitly reset mission after new mission / insertion --- src/client.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/client.py b/src/client.py index d9c0c9a..cbf9104 100644 --- a/src/client.py +++ b/src/client.py @@ -81,6 +81,8 @@ def upload_mission(wp_array): # Final ack MAV.setWPACK() + MAV.doCommand(MAVLink.MAV_CMD.DO_SET_MISSION_CURRENT, 0, 1, 0, 0, 0, 0, 0, 0) + #end = time.monotonic_ns() #print("Uploading mission took {:}ms".format((end - start) / 1000000)) From 1f8823198ccf464f95c419830ca70d805438ac21 Mon Sep 17 00:00:00 2001 From: Hansen Date: Mon, 10 Jun 2024 17:03:21 -0700 Subject: [PATCH 07/10] command-line option to disable GCOM-facing socket --- src/main.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main.py b/src/main.py index fb31cd9..3c26661 100644 --- a/src/main.py +++ b/src/main.py @@ -11,6 +11,7 @@ production = True HOST, PORT, SOCKET_PORT = "localhost", 9000, 9001 STATUS_HOST, STATUS_PORT = "localhost", 1323 +DISABLE_STATUS = False if __name__ == "__main__": # Extract arguments arguments = {} @@ -39,6 +40,9 @@ if '--status-port' in arguments.keys(): STATUS_PORT = arguments['--status-port'][0] + + if '--disable-status' in arguments.keys(): + DISABLE_STATUS = True print(f"Starting... HTTP server listening at {HOST}:{PORT}. Status WS connecting to {STATUS_HOST}:{STATUS_PORT}.") @@ -57,14 +61,17 @@ gcmh_thread = Thread(target=gcmh.serve_forever, args=[production, HOST, PORT]) #status websocket client thread - skth_thread = Thread(target=skth.connect_to, args=[production, STATUS_HOST, STATUS_PORT]) + if not DISABLE_STATUS: + skth_thread = Thread(target=skth.connect_to, args=[production, STATUS_HOST, STATUS_PORT]) print("\nStarting threads...\n") mpss_thread.start() gcmh_thread.start() - skth_thread.start() + if not DISABLE_STATUS: + skth_thread.start() mpss_thread.join() gcmh_thread.join() - skth_thread.join() + if not DISABLE_STATUS: + skth_thread.join() From 1fe14479fdc1c60462c562c0ac51fbeb242e1901 Mon Sep 17 00:00:00 2001 From: Hansen Date: Mon, 10 Jun 2024 17:12:50 -0700 Subject: [PATCH 08/10] change message as well --- src/main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.py b/src/main.py index 3c26661..58daeea 100644 --- a/src/main.py +++ b/src/main.py @@ -44,7 +44,7 @@ if '--disable-status' in arguments.keys(): DISABLE_STATUS = True - print(f"Starting... HTTP server listening at {HOST}:{PORT}. Status WS connecting to {STATUS_HOST}:{STATUS_PORT}.") + print(f"Starting... HTTP server listening at {HOST}:{PORT}. " + "" if DISABLE_STATUS else f"Status WS connecting to {STATUS_HOST}:{STATUS_PORT}.") # Instantiate shared object so = SharedObject() From 1f15d93d5f51df3aee812d880a8d95b2cf1dca7c Mon Sep 17 00:00:00 2001 From: Hansen Date: Tue, 11 Jun 2024 13:21:28 -0700 Subject: [PATCH 09/10] Updated README --- README.md | 10 ++++++++++ src/main.py | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index cc99b6b..f100ea4 100644 --- a/README.md +++ b/README.md @@ -92,6 +92,16 @@ Then, enter the src directory and run the `pytest` command via Poetry: poetry run pytest ``` +### Command Line Arguments + +| Argument | Description | +|-|-| +| `--dev` | If present, server is started in development mode rather than production. | +| `--port=9000` | Port on which to listen for HTTP requests. | +| `--status-host=localhost` | Hostname for the status socket to connect to. | +| `--status-port=1323` | Port for the status socket to connect to. | +| `--disable-status` | If present, disables the status socket. | + ## Endpoints See `api_spec.yml` or `postman_collection.json` for up-to-date information on endpoints. diff --git a/src/main.py b/src/main.py index 58daeea..363fd24 100644 --- a/src/main.py +++ b/src/main.py @@ -44,7 +44,7 @@ if '--disable-status' in arguments.keys(): DISABLE_STATUS = True - print(f"Starting... HTTP server listening at {HOST}:{PORT}. " + "" if DISABLE_STATUS else f"Status WS connecting to {STATUS_HOST}:{STATUS_PORT}.") + print(f"Starting... HTTP server listening at {HOST}:{PORT}. " + ("" if DISABLE_STATUS else f"Status WS connecting to {STATUS_HOST}:{STATUS_PORT}.")) # Instantiate shared object so = SharedObject() From 331d251c252dc66d401995a317f07f7835ea09fe Mon Sep 17 00:00:00 2001 From: ramjayakumar21 Date: Mon, 24 Jun 2024 14:54:55 -0700 Subject: [PATCH 10/10] add amin changes --- pyproject.toml | 1 + src/client.py | 2 +- src/server/common/status.py | 4 ++-- src/server/gcomhandler.py | 20 ++++++++++++++++++++ 4 files changed, 24 insertions(+), 3 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 63868c0..889ba16 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -18,6 +18,7 @@ gevent = "^23.9.1" gevent-websocket = "^0.10.1" requests = "^2.31.0" python-socketio = {extras = ["client"], version = "^5.11.2"} +flask-cors = "^4.0.1" [tool.poetry.group.dev.dependencies] pytest = "^7.4.4" diff --git a/src/client.py b/src/client.py index cbf9104..80c8584 100644 --- a/src/client.py +++ b/src/client.py @@ -109,7 +109,7 @@ def interpret_packedmission(recvd): # Send telemetry to server telemetry = b"TL" - telemetry += struct.pack('Qi12f', int(time.time() * 1000), int(cs.wpno), + telemetry += struct.pack('2i12f', int(time.time()), int(cs.wpno), cs.lat, cs.lng, cs.alt, cs.roll, cs.pitch, cs.yaw, cs.airspeed, cs.groundspeed, cs.verticalspeed, diff --git a/src/server/common/status.py b/src/server/common/status.py index 1352e82..412c8ce 100644 --- a/src/server/common/status.py +++ b/src/server/common/status.py @@ -29,7 +29,7 @@ def __init__(self, timestamp = 0, waypoint_number = -1, self._wvl: float = wind_velocity def encoded_status(self) -> bytes: - return struct.pack('Qi12f', + return struct.pack('2i12f', self._timestamp, self._wpn, self._lat, @@ -51,7 +51,7 @@ def decode_status(self, status_bytes: bytes) -> None: self._rol, self._pch, self._yaw, self._asp, self._gsp, self._vsp, self._btv, - self._wdr, self._wvl) = struct.unpack('Qi12f', status_bytes) + self._wdr, self._wvl) = struct.unpack('2i12f', status_bytes) def as_dictionary(self) -> dict: return { diff --git a/src/server/gcomhandler.py b/src/server/gcomhandler.py index cca43bf..1f8fd58 100644 --- a/src/server/gcomhandler.py +++ b/src/server/gcomhandler.py @@ -1,4 +1,5 @@ from flask import Flask, request +from flask_cors import CORS, cross_origin import json from shapely.geometry import Point, Polygon, MultiPoint, LineString from matplotlib import pyplot as plt @@ -33,14 +34,19 @@ def __init__(self, so): def serve_forever(self, production=True, HOST="localhost", PORT=9000): print("GCOM HTTP Server starting...") app = Flask(__name__) + CORS(app) + cors = CORS(app) + app.config['CORS_HEADERS'] = 'Content-Type' socketio = SocketIO(app) # GET endpoints @app.route("/", methods=["GET"]) + @cross_origin() def index(): return "GCOM Server Running", 200 @app.route("/queue", methods=["GET"]) + @cross_origin() def get_queue(): self._so.gcom_currentmission_trigger_update() while self._so._currentmission_flg_ready == False: @@ -62,6 +68,7 @@ def get_queue(): return retJSON @app.route("/status", methods=["GET"]) + @cross_origin() def get_status(): ret: Status = self._so.get_status() retJSON = json.dumps(ret.as_dictionary()) @@ -71,6 +78,7 @@ def get_status(): return retJSON @app.route("/land", methods=["GET"]) + @cross_origin() def land(): print("Landing") self._so.flightmode_set("loiter") @@ -79,6 +87,7 @@ def land(): return "Landing in Place", 200 @app.route("/rtl", methods=["GET", "POST"]) + @cross_origin() def rtl(): altitude = request.get_json().get('altitude', 50) @@ -90,6 +99,7 @@ def rtl(): # VTOL LAND ENDPOINT @app.route("/land", methods=["POST"]) + @cross_origin() def vtol_land(): land = request.get_json() if not 'latitude' in land or not 'longitude' in land: @@ -102,6 +112,7 @@ def vtol_land(): # POST endpoints @app.route("/queue", methods=["POST"]) + @cross_origin() def post_queue(): payload = request.get_json() @@ -136,6 +147,7 @@ def post_queue(): return "ok", 200 @app.route("/insert", methods=['POST']) + @cross_origin() def insert_wp(): payload = request.get_json() @@ -172,12 +184,14 @@ def insert_wp(): return "ok", 200 @app.route("/clear", methods=['GET']) + @cross_origin() def clear_queue(): self._so.gcom_newmission_set(WaypointQueue([])) return "ok", 200 @app.route("/takeoff", methods=["POST"]) + @cross_origin() def takeoff(): payload = request.get_json() @@ -199,6 +213,7 @@ def takeoff(): return "Takeoff unsuccessful", 400 @app.route("/home", methods=["POST"]) + @cross_origin() def home(): home = request.get_json() @@ -211,6 +226,7 @@ def home(): # VTOL endpoints @app.route("/vtol/transition", methods=["GET", "POST"]) + @cross_origin() def vtol_transition(): if request.method == "GET": return json.dumps({'mode': self._so.mps_vtol_get()}) @@ -228,6 +244,7 @@ def vtol_transition(): # FENCE DIVERSION METHOD (BIG BOY) @app.route("/diversion", methods=["POST"]) + @cross_origin() def fence_diversion(): self._so.gcom_locked_set(True) @@ -384,6 +401,7 @@ def fence_diversion(): return "diverting" @app.route("/flightmode", methods=["PUT"]) + @cross_origin() def change_flight_mode(): input = request.get_json() @@ -398,6 +416,7 @@ def change_flight_mode(): return f"Unrecognized mode: {input['mode']}", 400 @app.route("/arm", methods=["PUT"]) + @cross_origin() def arm_disarm_drone(): input = request.get_json() @@ -418,6 +437,7 @@ def arm_disarm_drone(): return f"Unrecognized arm/disarm command parameter", 400 @app.route("/altstandard", methods=["PUT"]) + @cross_origin() def altstandard(): #Call into altitude_standard_set return "UNIMPLMENTED", 410