[Work in Progress] Multiprocessing GUI

common/gui_interfaces/gui_interfaces/general/processing_gui.py

@@ -0,0 +1,79 @@
+import rclpy
+import threading
+import time
+import websocket
+import multiprocessing
+from src.manager.ram_logging.log_manager import LogManager
+
+class ProcessingGUI(multiprocessing.Process):
+
+    def __init__(self, host="ws://127.0.0.1:2303", freq=30.0):
+        super(ProcessingGUI, self).__init__()
+
+        # ROS 2 init
+        if not rclpy.ok():
+            rclpy.init()
+
+        # Execution control vars
+        self.out_period = 1.0 / freq
+
+        self.ack = True
+        self.ack_frontend = False
+        self.ack_lock = threading.Lock()
+
+        # self.client = RawValue(c_void_p, None)
+
+        self.running = True
+
+        self.host = host
+        self.node = rclpy.create_node("node")
+
+    def start_threads(self):
+        # 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):
+        while self.running:
+            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, incoming msgs can only be acks
+        if "ack" in message:
+            with self.ack_lock:
+                self.ack = True
+                self.ack_frontend = 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 map should be sent
+            with self.ack_lock:
+                if self.ack:
+                    self.update_gui()
+                    if self.ack_frontend: 
+                        self.ack = False
+
+            # Maintain desired frequency
+            elapsed = time.time() - start_time
+            sleep_time = max(0, self.out_period - elapsed)
+            time.sleep(sleep_time)
+
+    def send_to_client(self, msg):
+        if self.client:
+            try:
+                self.client.send(msg)
+            except Exception as e:
+                LogManager.logger.info(f"Error sending message: {e}")

exercises/static/exercises/follow_line_newmanager/python_template/ros2_humble/processingGUI.py

@@ -0,0 +1,104 @@
+#
+#
+#
+# THIS IS A PROTOTYPE, DO NOT USE
+#
+#
+#
+
+import cv2
+import base64
+import json
+import threading
+import rclpy
+from gui_interfaces.general.processing_gui import ProcessingGUI
+from console_interfaces.general.console import start_console
+from hal_interfaces.general.odometry import OdometryNode
+from lap import Lap
+from map import Map
+
+# Graphical User Interface Class
+
+class GUI(ProcessingGUI):
+
+    def __init__(self, host="ws://127.0.0.1:2303"):
+        super().__init__(host)
+
+        self.image_to_be_shown = None
+        self.image_to_be_shown_updated = False
+        self.image_show_lock = threading.Lock()
+
+        # Payload vars
+        self.payload = {'image': '','lap': '', 'map': ''}
+        self.circuit = "simple"
+        # TODO: maybe move this to HAL and have it be hybrid
+        pose3d_object = OdometryNode("/odom")
+        executor = rclpy.executors.MultiThreadedExecutor()
+        executor.add_node(pose3d_object)
+        executor_thread = threading.Thread(target=executor.spin, daemon=True)
+        executor_thread.start()
+        self.lap = Lap(pose3d_object)
+        self.map = Map(pose3d_object, self.circuit)
+
+        self.start_threads()
+
+    # Prepares and sends a map to the websocket server
+    def update_gui(self):
+
+        payload = self.payloadImage()
+        self.payload["image"] = json.dumps(payload)
+
+        # Payload Lap Message
+        lapped = self.lap.check_threshold()
+        self.payload["lap"] = ""
+        if(lapped != None):
+            self.payload["lap"] = str(lapped)
+
+        # Payload Map Message
+        pos_message = str(self.map.getFormulaCoordinates())
+        self.payload["map"] = pos_message
+
+        message = json.dumps(self.payload)
+        self.send_to_client(message)
+
+    # Function to prepare image payload
+    # Encodes the image as a JSON string and sends through the WS
+    def payloadImage(self):
+        with self.image_show_lock:
+            image_to_be_shown_updated = self.image_to_be_shown_updated
+            image_to_be_shown = self.image_to_be_shown
+
+        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
+
+        with self.image_show_lock:
+            self.image_to_be_shown_updated = False
+
+        return payload
+
+    # Function for student to call
+    def showImage(self, image):
+        with self.image_show_lock:
+            self.image_to_be_shown = image
+            self.image_to_be_shown_updated = True
+
+host = "ws://127.0.0.1:2303"
+gui = GUI(host)
+gui.start() # Needed to start the process
+
+# Redirect the console
+start_console()
+
+# Expose to the user
+def showImage(image):
+    gui.showImage(image)

exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/processingGUI.py

@@ -0,0 +1,150 @@
+#
+#
+#
+# THIS IS A PROTOTYPE, DO NOT USE
+#
+#
+#
+
+import json
+import cv2
+import base64
+import threading
+import time
+from gazebo_msgs.srv import SetEntityState, GetEntityState
+import rclpy
+from math import cos, sin, atan2
+
+from gui_interfaces.general.processing_gui import ProcessingGUI
+from gui_interfaces.general.threading_gui import ThreadingGUI
+from console_interfaces.general.console import start_console
+
+class GUI(ProcessingGUI):
+
+    def __init__(self, host="ws://127.0.0.1:2303", freq=30.0):
+        super(ProcessingGUI, self).__init__()
+        # ROS 2 init
+        if not rclpy.ok():
+            rclpy.init()
+
+        # Execution control vars
+        self.out_period = 1.0 / freq
+        self.image = None
+        self.image_lock = threading.Lock()
+        self.ack = True
+        self.ack_lock = threading.Lock()
+        self.running = True
+
+        self.host = host
+        self.msg = {"image": ""}
+        self.node = rclpy.create_node("node")
+
+        # Initialize the services
+        self.set_client = self.node.create_client(
+            SetEntityState, "/follow_person/set_entity_state"
+        )
+        while not self.set_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Service not available, waiting...")
+        self.set_request = SetEntityState.Request()
+
+        self.get_client = self.node.create_client(
+            GetEntityState, "/follow_person/get_entity_state"
+        )
+        while not self.get_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Service not available, waiting...")
+        self.get_request = GetEntityState.Request()
+
+        # Initialize and start the WebSocket client thread
+        self.start_threads()
+
+    # 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:
+            # Get the current pose
+            self.get_request.name = "PersonToControl"
+            self.get_request.reference_frame = "world"
+            get_future = self.get_client.call_async(self.get_request)
+            rclpy.spin_until_future_complete(self.node, get_future)
+            pose = get_future.result().state.pose
+
+            # Define movement and rotation parameters
+            mov_dist = 0.1  # meters (default for forward movement)
+            rot_angle = 0.17 # radians (default for left rotation)
+
+            # Check for movement direction 
+            if  "key_s" in message:     
+                mov_dist *= -1          # reverse for backward movement
+            if "key_d" in message:      
+                rot_angle *= -1         # reverse for right rotation
+
+            # Update accordingly
+            if "key_w" in message or "key_s" in message:   # forward or backward movement
+                siny_cosp = 2 * (pose.orientation.w * pose.orientation.z - pose.orientation.x * pose.orientation.y)
+                cosy_cosp = 1 - 2 * (pose.orientation.y * pose.orientation.y + pose.orientation.z * pose.orientation.z)
+                yaw = atan2(siny_cosp, cosy_cosp)
+                pose.position.x += mov_dist * sin(yaw)
+                pose.position.y += -mov_dist * cos(yaw)
+            elif "key_a" in message or "key_d" in message:  # turning movement
+                w = pose.orientation.w * cos(rot_angle / 2) - pose.orientation.z * sin(rot_angle / 2)
+                x = pose.orientation.x * cos(rot_angle / 2) + pose.orientation.y * sin(rot_angle / 2)
+                y = pose.orientation.y * cos(rot_angle / 2) - pose.orientation.x * sin(rot_angle / 2)
+                z = pose.orientation.w * sin(rot_angle / 2) + pose.orientation.z * cos(rot_angle / 2)
+                pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z = w, x, y, z
+
+            # Send the new pose
+            self.set_request.state.name = "PersonToControl"
+            self.set_request.state.pose = pose
+            self.set_request.state.reference_frame = "world"
+            set_future = self.set_client.call_async(self.set_request)
+            rclpy.spin_until_future_complete(self.node, set_future)
+
+    # 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 and self.image is not None:
+                        self.update_gui()
+                        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 update_gui(self):
+
+        _, encoded_image = cv2.imencode(".JPEG", self.image)
+        payload = {
+            "image": base64.b64encode(encoded_image).decode("utf-8"),
+            "shape": self.image.shape,
+        }
+        self.msg["image"] = json.dumps(payload)
+        message = json.dumps(self.msg)
+        self.send_to_client(message)
+
+    # Function to set the next image to be sent
+    def setImage(self, image):
+        with self.image_lock:
+            self.image = image
+
+
+host = "ws://127.0.0.1:2303"
+gui = GUI(host)
+gui.start()
+
+# Redirect the console
+start_console()
+
+# Expose the gui setImage function
+def showImage(img):
+    gui.setImage(img)