From 8d778f166f65df02570175be2a6543f97b411449 Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Mon, 13 May 2024 10:51:03 -0700 Subject: [PATCH] Fix reconnect behavior when rover code shuts down (#3) * Fix reconnect behavior when rover code shuts down * Fix wait forever behavior --- ArduPilotDriver.py | 107 +++++++++++++++++++++++---------------------- 1 file changed, 54 insertions(+), 53 deletions(-) diff --git a/ArduPilotDriver.py b/ArduPilotDriver.py index 94d28dd..47d1955 100755 --- a/ArduPilotDriver.py +++ b/ArduPilotDriver.py @@ -14,6 +14,7 @@ setattr(collections, "MutableMapping", collections.abc.MutableMapping) import dronekit + def debug_GPS_vars(): args = get_args() print("Waiting for connection to {}...".format(args.port)) @@ -30,14 +31,20 @@ def debug_GPS_vars(): print("lat: {}, lon: {}, roll: {}, pitch: {}, yaw: {}" .format(lat, lon, roll, pitch, yaw)) + def get_args(): parser = argparse.ArgumentParser() - parser.add_argument("-p", "--port", default="/dev/ttyACM10", help="Serial port connected to ardupilot") - parser.add_argument("-hz", "--frequency", type=float, default=10.0, help="Rate at which data is sent") - parser.add_argument("-b", "--baud", default=57600, type=int, help="Baud rate of serial connection") - parser.add_argument("--debug", action="store_true", help="Prints debug info to stdout") + parser.add_argument("-p", "--port", default="/dev/ttyACM10", + help="Serial port connected to ardupilot") + parser.add_argument("-hz", "--frequency", type=float, + default=10.0, help="Rate at which data is sent") + parser.add_argument("-b", "--baud", default=57600, + type=int, help="Baud rate of serial connection") + parser.add_argument("--debug", action="store_true", + help="Prints debug info to stdout") return parser.parse_args() + async def send_gps_message(websocket, lat, lon): msg = { "type": "gps", @@ -46,6 +53,7 @@ async def send_gps_message(websocket, lat, lon): } await websocket.send(json.dumps(msg)) + async def send_orientation_message(websocket, roll, pitch, yaw): msg = { "type": "orientation", @@ -55,12 +63,6 @@ async def send_orientation_message(websocket, roll, pitch, yaw): } await websocket.send(json.dumps(msg)) -async def send_heading_message(websocket, heading): - msg = { - "type": "heading", - "heading": heading - } - await websocket.send(json.dumps(msg)) async def async_main(): args = get_args() @@ -86,54 +88,53 @@ def cleanup(): ardupilot.parameters["COMPASS_ORIENT"] = 6 print("Connecting to rover server...") - async for websocket in websockets.connect("ws://localhost:3001/ardupilot"): - print("Done!") - def gps_callback(self, _, frame): - curr_time = time.perf_counter() - if curr_time >= last_data_send_times["gps"] + 1./args.frequency: - if args.debug: - print("GPS Fix:", ardupilot.gps_0.fix_type, frame) - if ardupilot.gps_0.fix_type > 1: - last_data_send_times["gps"] = curr_time - asyncio.run(send_gps_message(websocket, frame.lat, frame.lon)) - - def orientation_callback(self, _, attitude): - curr_time = time.perf_counter() - if curr_time >= last_data_send_times["orientation"] + 1./args.frequency: - if args.debug: - print(attitude) - else: - pitch_deg = attitude.pitch * 180 / math.pi - roll_deg = attitude.roll * 180 / math.pi - print(f"\33[2K\rroll={roll_deg: 4.0f}, pitch={pitch_deg: 4.0f}", end="") - last_data_send_times["orientation"] = curr_time - asyncio.run(send_orientation_message(websocket, attitude.roll, attitude.pitch, attitude.yaw)) - - def heading_callback(self, _, heading): - curr_time = time.perf_counter() - if curr_time >= last_data_send_times["heading"] + 1./args.frequency: - if args.debug: - print("Heading", heading) - last_data_send_times["heading"] = curr_time - asyncio.run(send_heading_message(websocket, heading)) - - ardupilot.add_attribute_listener("location.global_frame", gps_callback) - ardupilot.add_attribute_listener("attitude", orientation_callback) - # ardupilot.add_attribute_listener("heading", heading_callback) - + while True: try: - while True: - await asyncio.sleep(1) - except websockets.ConnectionClosed as e: - print(e) - ardupilot.remove_attribute_listener("location.global_frame", gps_callback) - ardupilot.remove_attribute_listener("attitude", orientation_callback) - # ardupilot.remove_attribute_listener("heading", heading_callback) - continue + async with websockets.connect("ws://localhost:3001/ardupilot") as websocket: + print("Done!") + + def gps_callback(self, _, frame): + curr_time = time.perf_counter() + if curr_time >= last_data_send_times["gps"] + 1./args.frequency: + if args.debug: + print("GPS Fix:", ardupilot.gps_0.fix_type, frame) + if ardupilot.gps_0.fix_type > 1: + last_data_send_times["gps"] = curr_time + asyncio.run(send_gps_message(websocket, frame.lat, frame.lon)) + + def orientation_callback(self, _, attitude): + curr_time = time.perf_counter() + if curr_time >= last_data_send_times["orientation"] + 1./args.frequency: + if args.debug: + print(attitude) + else: + pitch_deg = attitude.pitch * 180 / math.pi + roll_deg = attitude.roll * 180 / math.pi + print(f"\33[2K\rroll={roll_deg: 4.0f}, pitch={pitch_deg: 4.0f}", end="") + last_data_send_times["orientation"] = curr_time + asyncio.run(send_orientation_message( + websocket, attitude.roll, attitude.pitch, attitude.yaw)) + + ardupilot.add_attribute_listener("location.global_frame", gps_callback) + ardupilot.add_attribute_listener("attitude", orientation_callback) + + try: + await asyncio.Future() + except websockets.ConnectionClosed as e: + print(e) + ardupilot.remove_attribute_listener("location.global_frame", gps_callback) + ardupilot.remove_attribute_listener("attitude", orientation_callback) + print("Reconnecting to rover server...") + continue + except (websockets.WebSocketException, OSError): + await asyncio.sleep(3) + pass + def main(): debug_GPS_vars() + if __name__ == "__main__": asyncio.run(async_main()) # main()