From 6ee005d5d963b566c452442f4f0a5ee8592eab8e Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Fri, 2 Aug 2024 17:46:30 +0200 Subject: [PATCH 1/6] Adding base file --- .../gui_interfaces/general/processing_gui.py | 77 +++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100755 common/gui_interfaces/gui_interfaces/general/processing_gui.py diff --git a/common/gui_interfaces/gui_interfaces/general/processing_gui.py b/common/gui_interfaces/gui_interfaces/general/processing_gui.py new file mode 100755 index 000000000..161094028 --- /dev/null +++ b/common/gui_interfaces/gui_interfaces/general/processing_gui.py @@ -0,0 +1,77 @@ +import rclpy +import threading +import time +import websocket +from src.manager.ram_logging.log_manager import LogManager + +class ProcessingGUI: + + def __init__(self, host="ws://127.0.0.1:2303", freq=30.0): + + # 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(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}") From aa3b6d70bed31240b60451fa76748b4a9ecc89b6 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Fri, 2 Aug 2024 18:05:06 +0200 Subject: [PATCH 2/6] Trying first prototype --- .../gui_interfaces/gui_interfaces/general/processing_gui.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/common/gui_interfaces/gui_interfaces/general/processing_gui.py b/common/gui_interfaces/gui_interfaces/general/processing_gui.py index 161094028..be173a78f 100755 --- a/common/gui_interfaces/gui_interfaces/general/processing_gui.py +++ b/common/gui_interfaces/gui_interfaces/general/processing_gui.py @@ -2,11 +2,13 @@ import threading import time import websocket +import multiprocessing from src.manager.ram_logging.log_manager import LogManager -class ProcessingGUI: +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(): @@ -26,7 +28,7 @@ def __init__(self, host="ws://127.0.0.1:2303", freq=30.0): self.host = host self.node = rclpy.create_node("node") - def start(self): + def start_threads(self): # Initialize and start the WebSocket client thread threading.Thread(target=self.run_websocket, daemon=True).start() From 1009d2184eac50d38e896c07a2ad3e6ea5a7e531 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Fri, 2 Aug 2024 18:17:40 +0200 Subject: [PATCH 3/6] Prototype working for follow line --- .../ros2_humble/processingGUI.py | 104 ++++++++++++++++++ 1 file changed, 104 insertions(+) create mode 100755 exercises/static/exercises/follow_line_newmanager/python_template/ros2_humble/processingGUI.py diff --git a/exercises/static/exercises/follow_line_newmanager/python_template/ros2_humble/processingGUI.py b/exercises/static/exercises/follow_line_newmanager/python_template/ros2_humble/processingGUI.py new file mode 100755 index 000000000..d30309546 --- /dev/null +++ b/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) \ No newline at end of file From a358aa5db31e873c86242ebd37edb15df7be063e Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Sat, 3 Aug 2024 10:01:54 +0200 Subject: [PATCH 4/6] Changing ack handling --- .../gui_interfaces/gui_interfaces/general/processing_gui.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/gui_interfaces/gui_interfaces/general/processing_gui.py b/common/gui_interfaces/gui_interfaces/general/processing_gui.py index be173a78f..672bf6f1d 100755 --- a/common/gui_interfaces/gui_interfaces/general/processing_gui.py +++ b/common/gui_interfaces/gui_interfaces/general/processing_gui.py @@ -63,8 +63,8 @@ def gui_out_thread(self): with self.ack_lock: if self.ack: self.update_gui() - if self.ack_frontend: - self.ack = False + self.ack = False + # if self.ack_frontend: # Maintain desired frequency elapsed = time.time() - start_time From 255cc28e3b5108f0d729594f5d025adc396d0525 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Sat, 3 Aug 2024 10:09:13 +0200 Subject: [PATCH 5/6] Testing again --- .../gui_interfaces/gui_interfaces/general/processing_gui.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/gui_interfaces/gui_interfaces/general/processing_gui.py b/common/gui_interfaces/gui_interfaces/general/processing_gui.py index 672bf6f1d..be173a78f 100755 --- a/common/gui_interfaces/gui_interfaces/general/processing_gui.py +++ b/common/gui_interfaces/gui_interfaces/general/processing_gui.py @@ -63,8 +63,8 @@ def gui_out_thread(self): with self.ack_lock: if self.ack: self.update_gui() - self.ack = False - # if self.ack_frontend: + if self.ack_frontend: + self.ack = False # Maintain desired frequency elapsed = time.time() - start_time From 6479c564f2153306f6c50e423a0e87933ab48a38 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Mon, 5 Aug 2024 17:34:26 +0200 Subject: [PATCH 6/6] Two more prototypes for testing --- .../ros2_humble/processingGUI.py | 150 +++++++++++++++++ .../ros2_humble/processingGUI.py | 158 ++++++++++++++++++ 2 files changed, 308 insertions(+) create mode 100644 exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/processingGUI.py create mode 100644 exercises/static/exercises/global_navigation_newmanager/python_template/ros2_humble/processingGUI.py diff --git a/exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/processingGUI.py b/exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/processingGUI.py new file mode 100644 index 000000000..7518b401e --- /dev/null +++ b/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) diff --git a/exercises/static/exercises/global_navigation_newmanager/python_template/ros2_humble/processingGUI.py b/exercises/static/exercises/global_navigation_newmanager/python_template/ros2_humble/processingGUI.py new file mode 100644 index 000000000..bb24be34a --- /dev/null +++ b/exercises/static/exercises/global_navigation_newmanager/python_template/ros2_humble/processingGUI.py @@ -0,0 +1,158 @@ +# +# +# +# THIS IS A PROTOTYPE, DO NOT USE +# +# +# + +import cv2 +import base64 +import re +import json +import threading +import numpy as np + +from gui_interfaces.general.threading_gui import ThreadingGUI +from gui_interfaces.general.processing_gui import ProcessingGUI +from console_interfaces.general.console import start_console +from shared.image import SharedImage +from map import Map +from HAL import getPose3d + +# Graphical User Interface Class + +class GUI(ProcessingGUI): + + def __init__(self, host="ws://127.0.0.1:2303"): + super().__init__(host) + self.array = None + self.array_lock = threading.Lock() + self.mapXY = None + self.worldXY = None + + # Payload vars + self.payload = {'image': '', 'map': '', 'array': ''} + self.shared_image = SharedImage("numpyimage") + self.map = Map(getPose3d) + + self.start_threads() + + # 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 + elif "pick" in message: + data = eval(message[4:]) + self.mapXY = data + x, y = self.mapXY + self.worldXY = self.map.gridToWorld(x, y) + print(f"World : {self.worldXY}") + + # Prepares and sends a map to the websocket server + def update_gui(self): + + payload = self.payloadImage() + self.payload["image"] = json.dumps(payload) + + self.payload["array"] = self.array + # Payload Map Message + pos_message1 = self.map.getTaxiCoordinates() + ang_message = self.map.getTaxiAngle() + pos_message = str(pos_message1 + ang_message) + self.payload["map"] = pos_message + + message = json.dumps(self.payload) + self.send_to_client(message) + + def payloadImage(self): + """Encodes the image data to be sent to websocket""" + image = self.shared_image.get() + payload = {'image': '', 'shape': ''} + + shape = image.shape + frame = cv2.imencode('.PNG', image)[1] + encoded_image = base64.b64encode(frame) + + payload['image'] = encoded_image.decode('utf-8') + payload['shape'] = shape + + return payload + + def showNumpy(self, image): + processed_image = np.stack((image,) * 3, axis=-1) + self.shared_image.add(processed_image) + + def showPath(self, array): + """Process the array(ideal path) to be sent to websocket""" + with self.array_lock: + strArray = ''.join(str(e) for e in array) + + # Remove unnecessary spaces in the array to avoid JSON syntax error in JavaScript + strArray = re.sub(r"\[[ ]+", "[", strArray) + strArray = re.sub(r"[ ]+", ", ", strArray) + strArray = re.sub(r",[ ]+]", "]", strArray) + strArray = re.sub(r",,", ",", strArray) + strArray = re.sub(r"]\[", "],[", strArray) + strArray = "[" + strArray + "]" + + self.array = strArray + + def getTargetPose(self): + if self.worldXY is not None: + return self.worldXY + else: + return None + + def getMap(self, url): + return self.map.getMap(url) + + def worldToGrid(self, pose): + return self.map.worldToGrid(*pose) + + def gridToWorld(self, cell): + return self.map.gridToWorld(*cell) + + def reset_gui(self): + """Resets the GUI to its initial state.""" + print("Resetting image") + image = [[0 for x in range(400)] for y in range(400)] + self.showNumpy(np.clip(image, 0, 255).astype('uint8')) + self.map.reset() + +host = "ws://127.0.0.1:2303" +gui = GUI(host) +gui.start() + +# Redirect the console +start_console() + +# Expose to the user +def payloadImage(): + return gui.payloadImage() + +def showNumpy(image): + gui.showNumpy(image) + +def showPath(array): + gui.showPath(array) + +def getTargetPose(): + return gui.getTargetPose() + +def getMap(url): + return gui.getMap(url) + +def rowColumn(pose): + # Deprecated. Still alive for backward compatibility. + return list(gui.worldToGrid(pose)) + +def worldToGrid(pose): + return gui.worldToGrid(pose) + +def gridToWorld(cell): + return gui.gridToWorld(cell)