Initial status server test

python/examples/tcp_client.py

@@ -99,7 +99,7 @@
             for (header, message, raw_data) in messages:
                 if generating_p1log:
                     output_file.write(raw_data)
-
+                
                 if options.display:
                     print_message(header, message)
 

python/requirements.txt

@@ -16,3 +16,6 @@ scipy>=1.5.0
 
 # Required for development only.
 packaging>=21.0.0
+
+# Status server dependencies.
+structlog>=24.1.0

python/status_server/server.py

@@ -0,0 +1,143 @@
+import structlog
+import os
+from flask import Flask, jsonify, request
+from flask_cors import CORS
+import threading
+
+from datetime import datetime
+import os
+import socket
+import sys
+import json
+
+# Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports
+# if this application is being run directly out of the repository and is not installed as a pip package.
+root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..'))
+sys.path.insert(0, root_dir)
+
+from fusion_engine_client.parsers import FusionEngineDecoder
+from fusion_engine_client.utils.argument_parser import ArgumentParser
+
+from fusion_engine_client.messages import MessageHeader, MessageType, MessagePayload, message_type_to_class, PoseMessage
+
+from examples.message_decode import print_message
+
+
+class StoppableThread(threading.Thread):
+    def __init__(self, *args, **kwargs):
+        super(StoppableThread, self).__init__(*args, **kwargs)
+        self._stop_event = threading.Event()
+
+    def stop(self):
+        self._stop_event.set()
+
+    def stopped(self):
+        return self._stop_event.is_set()
+
+mutex = threading.Lock()
+
+logger = structlog.get_logger() 
+
+app = Flask(__name__)
+CORS(app)  # This will enable CORS for all routes
+
+latest_pose_message = {}
+
+@app.route('/', methods=['GET', 'POST'])
+def index():
+    global latest_pose_message
+    mutex.acquire()
+    latest_json_data = latest_pose_message
+    mutex.release()
+    return jsonify(latest_json_data), 200
+
+def start_fe_server(options):
+    output_file = None
+    global latest_pose_message
+    generating_raw_log = (output_file is not None and options.format == 'raw')
+    generating_p1log = (output_file is not None and options.format == 'p1log')
+
+    # Connect to the device.
+    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    sock.connect((socket.gethostbyname(options.hostname), options.port))
+    decoder = FusionEngineDecoder(warn_on_unrecognized=not options.quiet, return_bytes=True)
+    bytes_received = 0
+    messages_received = 0
+    start_time = datetime.now()
+    last_print_time = start_time
+    while not threading.current_thread().stopped():
+        # Read some data.
+        try:
+            received_data = sock.recv(1024)
+            bytes_received += len(received_data)
+
+            if not options.quiet:
+                now = datetime.now()
+                if (now - last_print_time).total_seconds() > 5.0:
+                    print('Status: [bytes_received=%d, messages_received=%d elapsed_time=%d sec]' %
+                          (bytes_received, messages_received, (now - start_time).total_seconds()))
+                    last_print_time = now
+        except KeyboardInterrupt:
+            break
+
+        # If logging in raw format, write the data to disk as is.
+        if generating_raw_log:
+            output_file.write(received_data)
+
+        # Decode the incoming data and print the contents of any complete messages.
+        #
+        # Note that we pass the data to the decoder, even if --no-display was requested, for three reasons:
+        # - So that we get a count of the number of incoming messages
+        # - So we print warnings if the CRC fails on any of the incoming data
+        # - If we are logging in *.p1log format, so the decoder can extract the FusionEngine data from any
+        #   non-FusionEngine data in the stream
+        messages = decoder.on_data(received_data)
+        messages_received += len(messages)
+
+        for (header, message, raw_data) in messages:
+            if isinstance(message, MessagePayload):
+                if message.get_type() == MessageType.POSE:
+                    # latest_pose_message = message
+                    mutex.acquire()
+                    pose_msg_np = PoseMessage.to_numpy([message])
+                    latest_pose_message["p1_time"] = float(pose_msg_np["p1_time"][0])
+                    latest_pose_message["gps_time"] = float(pose_msg_np["gps_time"][0])
+                    latest_pose_message["solution_type"] = float(pose_msg_np["solution_type"][0])
+                    latest_pose_message["latitude"] = float(pose_msg_np["lla_deg"][0][0])
+                    latest_pose_message["longitude"] = float(pose_msg_np["lla_deg"][1][0])
+                    latest_pose_message["altitude"] = float(pose_msg_np["lla_deg"][2][0])
+                    mutex.release()
+    # Close the socket.
+    sock.close()
+    logger.info("TCP server stopped", server_name="FE TCP Server")
+
+if __name__ == '__main__':
+
+    parser = ArgumentParser(description="""\
+Connect to an Point One device over TCP and print out the incoming message
+contents and/or log the messages to disk.
+""")
+
+    parser.add_argument('-f', '--format', default='p1log', choices=('p1log', 'raw'),
+                        help="The format of the file to be generated when --output is enabled."
+                             "If 'p1log' (default), create a *.p1log file containing only FusionEngine messages."
+                             "If 'raw', create a generic binary file containing all incoming data.")
+    parser.add_argument('-n', '--no-display', dest='display', action='store_false',
+                        help="Do not display the incoming message contents.")
+    parser.add_argument('-o', '--output', type=str,
+                        help="The path to a file where incoming data will be stored.")
+    parser.add_argument('-p', '--port', type=int, default=30201,
+                        help="The FusionEngine TCP port on the data source.")
+    parser.add_argument('-q', '--quiet', dest='quiet', action='store_true',
+                        help="Do not print anything to the console.")
+
+    parser.add_argument('hostname',
+                        help="The IP address or hostname of the data source.")
+
+    options = parser.parse_args()
+    fe_thread = StoppableThread(target=start_fe_server, args=(options,))
+    fe_thread.start()
+
+    app.run(port=4243)
+    fe_thread.stop()
+    fe_thread.join()