From 210114e0dcac750c167f69f1c9bec111f8ce2833 Mon Sep 17 00:00:00 2001 From: pariaspe Date: Wed, 3 Jul 2024 12:20:52 +0200 Subject: [PATCH 1/2] new ros2 version for follow_road exercise --- db.sqlite3 | Bin 327680 -> 327680 bytes .../python_template/ros2_humble/GUI.py | 129 +++++++++ .../python_template/ros2_humble/HAL.py | 115 ++++++++ .../python_template/ros2_humble/console.py | 19 ++ .../ros2_humble/gui_exercise.py | 248 ++++++++++++++++++ 5 files changed, 511 insertions(+) create mode 100644 exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/GUI.py create mode 100644 exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/HAL.py create mode 100644 exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/console.py create mode 100644 exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/gui_exercise.py diff --git a/db.sqlite3 b/db.sqlite3 index f37ee0ba066b61b28e5f6d54f8c1f2d9e9443f51..876dd98f84a6c9b71b3f3d2ff797fc960e0b47c0 100644 GIT binary patch delta 1413 zcmah}TTC2P7@l+4IXke+0K2fzEtX5U*4>?%y>Cn@1&ZY&7FrFo1r}%7a@n2b;)2u$ z3X4jUVw+Y^Vto-~n)JcaVlq$Fq!0RFLa0GajZu=u#6$_kG`?w^9csk3i7(&Df9CuC z|Nk=oe`aT`v$NJqcJT1>-g@xxR^Jy+)>h<3wSWo-aAAQx4@QC0dd!pfbfVkatBmsQwbfb3k4O4gwnnz+}wB$wcZI9a>0egzMb{RYHASfgXL zj#WBV>R6#;xjJ_r&auEt4Vy5+qNCtj)1sQ#gl_}Trg=IF==v`Zmqg>E$#5+4>Tq%( zF`e4nE&vubz#=jwMqI3ph&}_w9oh^OztBcdI8~k2 zl?^{lC-|Ihw;1#bw5}XgQXvTq^yUvZw_-R|rK@J1REZ%`bovFK=n?-_!DowU&MCad zJ&>wQ3muP@({|h=gd~sC6L9<7qU7E+O!QtS-bZ_?Q6_D_icB;0sS4fc0l;y)oTi@% zcSsbRfwC8V~@^E7S delta 558 zcmZ9HJ!n%=6vyAa=e~FI_3{$)VWO!-TTqcSFCSJ~OIxfW)PNw;p;ViONK9z+HHf(Q zhzQ!jB!+Uu!66_*2C0pBr&@5(twf1PT?~jzrC2%$f`b>bd8Whv_vgnshT%30ckM9R zsRyRf&a32yVZ}QTgGt2nQ^fD{djLW>om4N{XivDD#gxWtzo6s@nKmRBN=Wt&9#?nOi0!;JZCN!x zM+uXcHt7lFr4pCl%U8%9*a9LxKZ1Ayky*u3o-|3LgV(10>S~?Lv1bZ#aIcB@OyZme zDt|LTZXXb9+|E-Iq*;f6tc$bAsL1vR*FTf4#cP&ZlJ%zLoH;;$D;@h+^|kGh+!BqN K%v(Rm?4f^16`rgB diff --git a/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/GUI.py b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/GUI.py new file mode 100644 index 000000000..bcbe995a9 --- /dev/null +++ b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/GUI.py @@ -0,0 +1,129 @@ +import json +import cv2 +import base64 +import threading +import time +import websocket +from src.manager.ram_logging.log_manager import LogManager +from console import start_console +import numpy as np + + +class ThreadingGUI: + + def __init__(self, host="ws://127.0.0.1:2303", freq=30.0): + + # Execution control vars + self.out_period = 1.0 / freq + self.right_image = None + self.left_image = None + self.image_lock = threading.Lock() + self.ack = True + self.ack_lock = threading.Lock() + self.running = True + + self.host = host + self.msg = {"image_right": "", "image_left": ""} + + # Initialize and start the WebSocket client thread + threading.Thread(target=self.run_websocket, daemon=True).start() + + # Initialize and start the image sending thread (GUI out thread) + threading.Thread( + target=self.gui_out_thread, name="gui_out_thread", daemon=True + ).start() + + # Init websocket client + def run_websocket(self): + self.client = websocket.WebSocketApp(self.host, on_message=self.gui_in_thread) + self.client.run_forever(ping_timeout=None, ping_interval=0) + + # Process incoming messages to the GUI + def gui_in_thread(self, ws, message): + + # In this case, messages can be either acks or key strokes + if "ack" in message: + with self.ack_lock: + self.ack = True + else: + LogManager.logger.error("Unsupported msg") + + # Process outcoming messages from the GUI + def gui_out_thread(self): + while self.running: + start_time = time.time() + + # Check if a new image should be sent + with self.ack_lock: + with self.image_lock: + if self.ack: + if np.any(self.left_image) or np.any(self.right_image): + self.send_images() + self.ack = False + + # Maintain desired frequency + elapsed = time.time() - start_time + sleep_time = max(0, self.out_period - elapsed) + time.sleep(sleep_time) + + # Prepares and send image to the websocket server + def send_images(self): + + if np.any(self.left_image): + _, encoded_left_image = cv2.imencode(".JPEG", self.left_image) + b64_left = base64.b64encode(encoded_left_image).decode("utf-8") + shape_left = self.left_image.shape + else: + b64_left = None + shape_left = 0 + + if np.any(self.right_image): + _, encoded_right_image = cv2.imencode(".JPEG", self.right_image) + b64_right = base64.b64encode(encoded_right_image).decode("utf-8") + shape_right = self.right_image.shape + else: + b64_right = None + shape_right = 0 + + payload_left = { + "image_left": b64_left, + "shape_left": shape_left, + } + payload_right = { + "image_right": b64_right, + "shape_right": shape_right, + } + self.msg["image_left"] = json.dumps(payload_left) + self.msg["image_right"] = json.dumps(payload_right) + message = json.dumps(self.msg) + try: + if self.client: + self.client.send(message) + except Exception as e: + LogManager.logger.info(f"Error sending message: {e}") + + # Functions to set the next image to be sent + def setLeftImage(self, image): + with self.image_lock: + self.left_image = image + + def setRightImage(self, image): + with self.image_lock: + self.right_image = image + + +host = "ws://127.0.0.1:2303" +gui = ThreadingGUI(host) + + +# Redirect the console +start_console() + + +# Expose the user functions +def showImage(image): + gui.setRightImage(image) + + +def showLeftImage(image): + gui.setLeftImage(image) diff --git a/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/HAL.py b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/HAL.py new file mode 100644 index 000000000..88fd85826 --- /dev/null +++ b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/HAL.py @@ -0,0 +1,115 @@ +import numpy as np +import rclpy +import threading +import time + +from hal_interfaces.general.camera import CameraNode +from jderobot_drones.drone_wrapper import DroneWrapper + +IMG_WIDTH = 320 +IMG_HEIGHT = 240 +freq = 15.0 + +### HAL INIT ### + +print("HAL initializing", flush=True) +if not rclpy.ok(): + rclpy.init() + + + CAM_FRONTAL_TOPIC = "/" + "drone0" + "/sensor_measurements/frontal_camera/image_raw" + CAM_VENTRAL_TOPIC = "/" + "drone0" + "/sensor_measurements/ventral_camera/image_raw" + + drone = DroneWrapper() + frontal_camera_node = CameraNode(CAM_FRONTAL_TOPIC) + ventral_camera_node = CameraNode(CAM_VENTRAL_TOPIC) + + # Spin nodes so that subscription callbacks load topic data + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(frontal_camera_node) + executor.add_node(ventral_camera_node) + def __auto_spin() -> None: + while rclpy.ok(): + executor.spin_once(timeout_sec=0) + time.sleep(1/freq) + executor_thread = threading.Thread(target=__auto_spin, daemon=True) + executor_thread.start() + +### GETTERS ### + + +def get_frontal_image(): + image = frontal_camera_node.getImage() + while image == None: + image = frontal_camera_node.getImage() + return image.data + + +def get_ventral_image(): + image = ventral_camera_node.getImage() + while image == None: + image = ventral_camera_node.getImage() + return image.data + + +def get_position(): + pos = drone.get_position() + return pos + + +def get_velocity(): + vel = drone.get_velocity() + return vel + + +def get_yaw_rate(): + yaw_rate = drone.get_yaw_rate() + return yaw_rate + + +def get_orientation(): + orientation = drone.get_orientation() + return orientation + + +def get_roll(): + roll = drone.get_roll() + return roll + + +def get_pitch(): + pitch = drone.get_pitch() + return pitch + + +def get_yaw(): + yaw = drone.get_yaw() + return yaw + + +def get_landed_state(): + state = drone.get_landed_state() + return state + + +### SETTERS ### + + +def set_cmd_pos(x, y, z, az): + drone.set_cmd_pos(x, y, z, az) + + +def set_cmd_vel(vx, vy, vz, az): + drone.set_cmd_vel(vx, vy, vz, az) + + +def set_cmd_mix(vx, vy, z, az): + drone.set_cmd_mix(vx, vy, z, az) + + +def takeoff(h=3): + drone.takeoff(h) + + +def land(): + drone.land() diff --git a/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/console.py b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/console.py new file mode 100644 index 000000000..3c88c9d71 --- /dev/null +++ b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/console.py @@ -0,0 +1,19 @@ +# Functions to start and close console +import os +import sys + + +def start_console(): + # Get all the file descriptors and choose the latest one + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) + + sys.stderr = open('/dev/pts/' + console_fd, 'w') + sys.stdout = open('/dev/pts/' + console_fd, 'w') + sys.stdin = open('/dev/pts/' + console_fd, 'w') + + +def close_console(): + sys.stderr.close() + sys.stdout.close() + sys.stdin.close() diff --git a/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/gui_exercise.py b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/gui_exercise.py new file mode 100644 index 000000000..6723447b3 --- /dev/null +++ b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/gui_exercise.py @@ -0,0 +1,248 @@ +import json +import cv2 +import base64 +import threading +import time +from datetime import datetime +import websocket +import subprocess + + +# Graphical User Interface Class +class GUI: + # Initialization function + # The actual initialization + def __init__(self, host): + + self.payload = {"image": ""} + self.left_payload = {"image_left": ""} + self.server = None + self.client = None + + self.host = host + + # Image variables + self.image_to_be_shown = None + self.image_to_be_shown_updated = False + self.image_show_lock = threading.Lock() + + self.left_image_to_be_shown = None + self.left_image_to_be_shown_updated = False + self.left_image_show_lock = threading.Lock() + + self.acknowledge = False + self.acknowledge_lock = threading.Lock() + + self.client_thread = threading.Thread(target=self.run_websocket) + self.client_thread.start() + + def run_websocket(self): + while True: + self.client = websocket.WebSocketApp( + "ws://127.0.0.1:2303", + on_message=self.on_message, + ) + self.client.run_forever(ping_timeout=None, ping_interval=0) + + # Explicit initialization function + # Class method, so user can call it without instantiation + @classmethod + def initGUI(cls, host): + # self.payload = {'image': '', 'shape': []} + new_instance = cls(host) + return new_instance + + # Function to prepare image payload + # Encodes the image as a JSON string and sends through the WS + def payloadImage(self): + self.image_show_lock.acquire() + image_to_be_shown_updated = self.image_to_be_shown_updated + image_to_be_shown = self.image_to_be_shown + self.image_show_lock.release() + + image = image_to_be_shown + payload = {"image": "", "shape": ""} + + if not image_to_be_shown_updated: + return payload + + shape = image.shape + frame = cv2.imencode(".JPEG", image)[1] + encoded_image = base64.b64encode(frame) + + payload["image"] = encoded_image.decode("utf-8") + payload["shape"] = shape + + self.image_show_lock.acquire() + self.image_to_be_shown_updated = False + self.image_show_lock.release() + + return payload + + # Function to prepare image payload + # Encodes the image as a JSON string and sends through the WS + def payloadLeftImage(self): + self.left_image_show_lock.acquire() + left_image_to_be_shown_updated = self.left_image_to_be_shown_updated + left_image_to_be_shown = self.left_image_to_be_shown + self.left_image_show_lock.release() + + image = left_image_to_be_shown + payload = {"image_left": "", "shape": ""} + + if not left_image_to_be_shown_updated: + return payload + + shape = image.shape + frame = cv2.imencode(".JPEG", image)[1] + encoded_image = base64.b64encode(frame) + + payload["image_left"] = encoded_image.decode("utf-8") + payload["shape"] = shape + + self.left_image_show_lock.acquire() + self.left_image_to_be_shown_updated = False + self.left_image_show_lock.release() + + return payload + + # Function for student to call + def showImage(self, image): + self.image_show_lock.acquire() + self.image_to_be_shown = image + self.image_to_be_shown_updated = True + self.image_show_lock.release() + + # Function for student to call + def showLeftImage(self, image): + self.left_image_show_lock.acquire() + self.left_image_to_be_shown = image + self.left_image_to_be_shown_updated = True + self.left_image_show_lock.release() + + # Function to get the client + # Called when a new client is received + def get_client(self, client, server): + self.client = client + + # Function to get value of Acknowledge + def get_acknowledge(self): + self.acknowledge_lock.acquire() + acknowledge = self.acknowledge + self.acknowledge_lock.release() + + return acknowledge + + # Function to get value of Acknowledge + def set_acknowledge(self, value): + self.acknowledge_lock.acquire() + self.acknowledge = value + self.acknowledge_lock.release() + + # Update the gui + def update_gui(self): + # Payload Image Message + payload = self.payloadImage() + self.payload["image"] = json.dumps(payload) + + message = json.dumps(self.payload) + if self.client: + try: + self.client.send(message) + except Exception as e: + print(f"Error sending message: {e}") + + # Payload Left Image Message + left_payload = self.payloadLeftImage() + self.left_payload["image_left"] = json.dumps(left_payload) + + message = json.dumps(self.left_payload) + if self.client: + try: + self.client.send(message) + except Exception as e: + print(f"Error sending message: {e}") + + def on_message(self, ws, message): + """Handles incoming messages from the websocket client.""" + if message.startswith("#ack"): + self.set_acknowledge(True) + + # Function to reset + def reset_gui(self): + pass + + +class ThreadGUI: + """Class to manage GUI updates and frequency measurements in separate threads.""" + + def __init__(self, gui): + """Initializes the ThreadGUI with a reference to the GUI instance.""" + self.gui = gui + self.ideal_cycle = 80 + self.real_time_factor = 0 + self.frequency_message = {"brain": "", "gui": "", "rtf": ""} + self.iteration_counter = 0 + self.running = True + + def start(self): + """Starts the GUI, frequency measurement, and real-time factor threads.""" + self.frequency_thread = threading.Thread(target=self.measure_and_send_frequency) + self.gui_thread = threading.Thread(target=self.run) + self.rtf_thread = threading.Thread(target=self.get_real_time_factor) + self.frequency_thread.start() + self.gui_thread.start() + self.rtf_thread.start() + print("GUI Thread Started!") + + def get_real_time_factor(self): + """Continuously calculates the real-time factor.""" + while True: + time.sleep(2) + args = ["gz", "stats", "-p"] + stats_process = subprocess.Popen(args, stdout=subprocess.PIPE) + with stats_process.stdout: + for line in iter(stats_process.stdout.readline, b""): + stats_list = [x.strip() for x in line.split(b",")] + self.real_time_factor = stats_list[0].decode("utf-8") + + def measure_and_send_frequency(self): + """Measures and sends the frequency of GUI updates and brain cycles.""" + previous_time = datetime.now() + while self.running: + time.sleep(2) + current_time = datetime.now() + dt = current_time - previous_time + ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 + previous_time = current_time + measured_cycle = ( + ms / self.iteration_counter if self.iteration_counter > 0 else 0 + ) + self.iteration_counter = 0 + brain_frequency = ( + round(1000 / measured_cycle, 1) if measured_cycle != 0 else 0 + ) + gui_frequency = round(1000 / self.ideal_cycle, 1) + self.frequency_message = { + "brain": brain_frequency, + "gui": gui_frequency, + "rtf": self.real_time_factor, + } + message = json.dumps(self.frequency_message) + if self.gui.client: + try: + self.gui.client.send(message) + except Exception as e: + print(f"Error sending frequency message: {e}") + + def run(self): + """Main loop to update the GUI at regular intervals.""" + while self.running: + start_time = datetime.now() + self.gui.update_gui() + self.iteration_counter += 1 + finish_time = datetime.now() + dt = finish_time - start_time + ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 + sleep_time = max(0, (50 - ms) / 1000.0) + time.sleep(sleep_time) From c5949ff8de7609679a62495530f2f9cc0fec8da1 Mon Sep 17 00:00:00 2001 From: pariaspe Date: Wed, 3 Jul 2024 16:50:28 +0200 Subject: [PATCH 2/2] remove gui_exercise.py --- .../ros2_humble/gui_exercise.py | 248 ------------------ 1 file changed, 248 deletions(-) delete mode 100644 exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/gui_exercise.py diff --git a/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/gui_exercise.py b/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/gui_exercise.py deleted file mode 100644 index 6723447b3..000000000 --- a/exercises/static/exercises/follow_road_newmanager/python_template/ros2_humble/gui_exercise.py +++ /dev/null @@ -1,248 +0,0 @@ -import json -import cv2 -import base64 -import threading -import time -from datetime import datetime -import websocket -import subprocess - - -# Graphical User Interface Class -class GUI: - # Initialization function - # The actual initialization - def __init__(self, host): - - self.payload = {"image": ""} - self.left_payload = {"image_left": ""} - self.server = None - self.client = None - - self.host = host - - # Image variables - self.image_to_be_shown = None - self.image_to_be_shown_updated = False - self.image_show_lock = threading.Lock() - - self.left_image_to_be_shown = None - self.left_image_to_be_shown_updated = False - self.left_image_show_lock = threading.Lock() - - self.acknowledge = False - self.acknowledge_lock = threading.Lock() - - self.client_thread = threading.Thread(target=self.run_websocket) - self.client_thread.start() - - def run_websocket(self): - while True: - self.client = websocket.WebSocketApp( - "ws://127.0.0.1:2303", - on_message=self.on_message, - ) - self.client.run_forever(ping_timeout=None, ping_interval=0) - - # Explicit initialization function - # Class method, so user can call it without instantiation - @classmethod - def initGUI(cls, host): - # self.payload = {'image': '', 'shape': []} - new_instance = cls(host) - return new_instance - - # Function to prepare image payload - # Encodes the image as a JSON string and sends through the WS - def payloadImage(self): - self.image_show_lock.acquire() - image_to_be_shown_updated = self.image_to_be_shown_updated - image_to_be_shown = self.image_to_be_shown - self.image_show_lock.release() - - image = image_to_be_shown - payload = {"image": "", "shape": ""} - - if not image_to_be_shown_updated: - return payload - - shape = image.shape - frame = cv2.imencode(".JPEG", image)[1] - encoded_image = base64.b64encode(frame) - - payload["image"] = encoded_image.decode("utf-8") - payload["shape"] = shape - - self.image_show_lock.acquire() - self.image_to_be_shown_updated = False - self.image_show_lock.release() - - return payload - - # Function to prepare image payload - # Encodes the image as a JSON string and sends through the WS - def payloadLeftImage(self): - self.left_image_show_lock.acquire() - left_image_to_be_shown_updated = self.left_image_to_be_shown_updated - left_image_to_be_shown = self.left_image_to_be_shown - self.left_image_show_lock.release() - - image = left_image_to_be_shown - payload = {"image_left": "", "shape": ""} - - if not left_image_to_be_shown_updated: - return payload - - shape = image.shape - frame = cv2.imencode(".JPEG", image)[1] - encoded_image = base64.b64encode(frame) - - payload["image_left"] = encoded_image.decode("utf-8") - payload["shape"] = shape - - self.left_image_show_lock.acquire() - self.left_image_to_be_shown_updated = False - self.left_image_show_lock.release() - - return payload - - # Function for student to call - def showImage(self, image): - self.image_show_lock.acquire() - self.image_to_be_shown = image - self.image_to_be_shown_updated = True - self.image_show_lock.release() - - # Function for student to call - def showLeftImage(self, image): - self.left_image_show_lock.acquire() - self.left_image_to_be_shown = image - self.left_image_to_be_shown_updated = True - self.left_image_show_lock.release() - - # Function to get the client - # Called when a new client is received - def get_client(self, client, server): - self.client = client - - # Function to get value of Acknowledge - def get_acknowledge(self): - self.acknowledge_lock.acquire() - acknowledge = self.acknowledge - self.acknowledge_lock.release() - - return acknowledge - - # Function to get value of Acknowledge - def set_acknowledge(self, value): - self.acknowledge_lock.acquire() - self.acknowledge = value - self.acknowledge_lock.release() - - # Update the gui - def update_gui(self): - # Payload Image Message - payload = self.payloadImage() - self.payload["image"] = json.dumps(payload) - - message = json.dumps(self.payload) - if self.client: - try: - self.client.send(message) - except Exception as e: - print(f"Error sending message: {e}") - - # Payload Left Image Message - left_payload = self.payloadLeftImage() - self.left_payload["image_left"] = json.dumps(left_payload) - - message = json.dumps(self.left_payload) - if self.client: - try: - self.client.send(message) - except Exception as e: - print(f"Error sending message: {e}") - - def on_message(self, ws, message): - """Handles incoming messages from the websocket client.""" - if message.startswith("#ack"): - self.set_acknowledge(True) - - # Function to reset - def reset_gui(self): - pass - - -class ThreadGUI: - """Class to manage GUI updates and frequency measurements in separate threads.""" - - def __init__(self, gui): - """Initializes the ThreadGUI with a reference to the GUI instance.""" - self.gui = gui - self.ideal_cycle = 80 - self.real_time_factor = 0 - self.frequency_message = {"brain": "", "gui": "", "rtf": ""} - self.iteration_counter = 0 - self.running = True - - def start(self): - """Starts the GUI, frequency measurement, and real-time factor threads.""" - self.frequency_thread = threading.Thread(target=self.measure_and_send_frequency) - self.gui_thread = threading.Thread(target=self.run) - self.rtf_thread = threading.Thread(target=self.get_real_time_factor) - self.frequency_thread.start() - self.gui_thread.start() - self.rtf_thread.start() - print("GUI Thread Started!") - - def get_real_time_factor(self): - """Continuously calculates the real-time factor.""" - while True: - time.sleep(2) - args = ["gz", "stats", "-p"] - stats_process = subprocess.Popen(args, stdout=subprocess.PIPE) - with stats_process.stdout: - for line in iter(stats_process.stdout.readline, b""): - stats_list = [x.strip() for x in line.split(b",")] - self.real_time_factor = stats_list[0].decode("utf-8") - - def measure_and_send_frequency(self): - """Measures and sends the frequency of GUI updates and brain cycles.""" - previous_time = datetime.now() - while self.running: - time.sleep(2) - current_time = datetime.now() - dt = current_time - previous_time - ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 - previous_time = current_time - measured_cycle = ( - ms / self.iteration_counter if self.iteration_counter > 0 else 0 - ) - self.iteration_counter = 0 - brain_frequency = ( - round(1000 / measured_cycle, 1) if measured_cycle != 0 else 0 - ) - gui_frequency = round(1000 / self.ideal_cycle, 1) - self.frequency_message = { - "brain": brain_frequency, - "gui": gui_frequency, - "rtf": self.real_time_factor, - } - message = json.dumps(self.frequency_message) - if self.gui.client: - try: - self.gui.client.send(message) - except Exception as e: - print(f"Error sending frequency message: {e}") - - def run(self): - """Main loop to update the GUI at regular intervals.""" - while self.running: - start_time = datetime.now() - self.gui.update_gui() - self.iteration_counter += 1 - finish_time = datetime.now() - dt = finish_time - start_time - ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 - sleep_time = max(0, (50 - ms) / 1000.0) - time.sleep(sleep_time)