From 03c88d52013d192fb3d3dc9f527a8f6a1820480f Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 May 2024 19:07:35 +0200 Subject: [PATCH 01/12] First try modifying files --- .../console.py | 0 .../exercise.py | 0 .../{ros1_noetic => noetic_deprecated}/gui.py | 0 .../{ros1_noetic => noetic_deprecated}/hal.py | 0 .../interfaces/__init__.py | 0 .../interfaces/bumper.py | 0 .../interfaces/laser.py | 0 .../interfaces/motors.py | 0 .../interfaces/pose3d.py | 0 .../interfaces/threadPublisher.py | 0 .../{ros1_noetic => noetic_deprecated}/map.py | 0 .../shared/__init__.py | 0 .../shared/image.py | 0 .../shared/structure_img.py | 0 .../shared/value.py | 0 .../python_template/ros2_humble/GUI.py | 192 ++++++++ .../python_template/ros2_humble/HAL.py | 69 +++ .../python_template/ros2_humble/exercise.py | 415 ------------------ .../python_template/ros2_humble/gui.py | 245 ----------- .../python_template/ros2_humble/hal.py | 58 --- .../ros2_humble/interfaces/__init__.py | 0 .../ros2_humble/interfaces/bumper.py | 166 ------- .../ros2_humble/interfaces/laser.py | 116 ----- .../ros2_humble/interfaces/motors.py | 121 ----- .../ros2_humble/interfaces/pose3d.py | 179 -------- .../ros2_humble/interfaces/threadPublisher.py | 46 -- .../python_template/ros2_humble/map.py | 30 +- 27 files changed, 285 insertions(+), 1352 deletions(-) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/console.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/exercise.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/gui.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/hal.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/interfaces/__init__.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/interfaces/bumper.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/interfaces/laser.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/interfaces/motors.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/interfaces/pose3d.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/interfaces/threadPublisher.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/map.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/shared/__init__.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/shared/image.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/shared/structure_img.py (100%) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/{ros1_noetic => noetic_deprecated}/shared/value.py (100%) create mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py create mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/exercise.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/gui.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/hal.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/__init__.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/bumper.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/laser.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/motors.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/pose3d.py delete mode 100755 exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/threadPublisher.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/console.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/console.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/console.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/console.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/exercise.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/exercise.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/exercise.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/exercise.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/gui.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/gui.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/gui.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/gui.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/hal.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/hal.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/hal.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/hal.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/__init__.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/__init__.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/__init__.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/__init__.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/bumper.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/bumper.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/bumper.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/bumper.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/laser.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/laser.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/laser.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/laser.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/motors.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/motors.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/motors.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/motors.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/pose3d.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/pose3d.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/pose3d.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/pose3d.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/threadPublisher.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/threadPublisher.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/threadPublisher.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/interfaces/threadPublisher.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/map.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/map.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/map.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/map.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/shared/__init__.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/shared/__init__.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/shared/__init__.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/shared/__init__.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/shared/image.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/shared/image.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/shared/image.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/shared/image.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/shared/structure_img.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/shared/structure_img.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/shared/structure_img.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/shared/structure_img.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/shared/value.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/shared/value.py similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/shared/value.py rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/noetic_deprecated/shared/value.py diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py new file mode 100755 index 000000000..e970251dc --- /dev/null +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py @@ -0,0 +1,192 @@ +import json +import cv2 +import base64 +import threading +import time +import websocket +from src.manager.ram_logging.log_manager import LogManager +from gazebo_msgs.srv import SetEntityState, GetEntityState +from shared.image import SharedImage +import matplotlib.pyplot as plt +import rclpy +from console import start_console +from map import Map +from HAL import getPose3d + +from map import Map + +# Graphical User Interface Class + +# Matrix colors +red = [0, 0, 255] +orange = [0, 165, 255] +yellow = [0, 255, 255] +green = [0, 255, 0] +blue = [255, 0, 0] +indigo = [130, 0, 75] +violet = [211, 0, 148] + +class ThreadingGUI: + + 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_lock = threading.Lock() + self.running = True + self.host = host + self.node = rclpy.create_node("node") + + # Payload vars + # self.msg = {"pos_msg": "", "ang_msg": ""} + self.msg = {'map': '', 'nav': ''} + self.init_coords = (171, 63) + self.start_coords = (201, 85.5) + self.map = Map(getPose3d) + + # Image vars + self.shared_image = SharedImage("guiimage") + + # 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, incoming msgs can only be acks + if "ack" in message: + with self.ack_lock: + self.ack = True + + # 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 and self.map 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) + + # Update the gui + def update_gui(self): + # Payload Map Message + pos_message = self.map.getRobotCoordinates() + if (pos_message == self.init_coords): + pos_message = self.start_coords + ang_message = self.map.getRobotAngle() + pos_message = str(pos_message + ang_message) + self.msg["map"] = pos_message + + # Example Payload Navigation Data message (random data) + # 4 colors supported (0, 1, 2, 3) + #nav_mat = np.zeros((20, 20), int) + #nav_mat[2, 1] = 1 + #nav_mat[3, 3] = 2 + #nav_mat[5,9] = 3 + #nav_message = str(nav_mat.tolist()) + payload = self.payloadImage() + self.msg["image"] = json.dumps(payload) + + message = json.dumps(self.msg) + if self.client: + try: + self.client.send(message) + except Exception as e: + print(f"Error sending message: {e}") + + + # Prepares and sends a map to the websocket server + # def send_map(self): + + # # Get the necessary info + # pos_message = self.map.getRobotCoordinates() + # if pos_message == self.init_coords: + # pos_message = self.start_coords + # ang_message = self.map.getRobotAngle() + + # self.msg["pos_msg"] = pos_message + # self.msg["ang_msg"] = ang_message + # 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}") + + # encode the image data to be sent to websocket + def payloadImage(self): + + 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 process_colors(self, image): + colored_image = np.zeros((image.shape[0], image.shape[1], 3), dtype=np.uint8) + + # Grayscale for values < 128 + mask = image < 128 + colored_image[mask] = image[mask][:, None] * 2 + + # Color lookup table + color_table = { + 128: red, + 129: orange, + 130: yellow, + 131: green, + 132: blue, + 133: indigo, + 134: violet + } + + for value, color in color_table.items(): + mask = image == value + colored_image[mask] = color + + return colored_image + + # load the image data + def showNumpy(self, image): + self.shared_image.add(self.process_colors(image)) + + def getMap(self, url): + return plt.imread(url) + + +host = "ws://127.0.0.1:2303" +gui = ThreadingGUI(host) + +def showNumpy(image): + gui.show(image) + +# Redirect the console +start_console() \ No newline at end of file diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py new file mode 100755 index 000000000..9aebdc5ba --- /dev/null +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py @@ -0,0 +1,69 @@ +import rclpy +import sys +from console import start_console + +from hal_interfaces.general.motors import MotorsNode +from hal_interfaces.general.odometry import OdometryNode +from hal_interfaces.general.laser import LaserNode +from hal_interfaces.general.bumper import BumperNode + +### HAL INIT ### + +print("HAL initializing", flush=True) +if not rclpy.ok(): + rclpy.init(args=sys.argv) + +motor_node = MotorsNode("/cmd_vel", 4, 0.3) +odometry_node = OdometryNode("/odom") # TODO: check if correct or "/gazebo/model_states" +laser_node = LaserNode("/roombaROS/laser/scan") +bumper_node = BumperNode( + [ + "/roombaROS/events/right_bumper", + "/roombaROS/events/center_bumper", + "/roombaROS/events/left_bumper", + ] +) + +### GETTERS ### + + +# Laser +def getLaserData(): + try: + rclpy.spin_once(laser_node) + values = laser_node.getLaserData().values + return values + except Exception as e: + print(f"Exception in hal getLaserData {repr(e)}") + + +# Pose +def getPose3d(): + try: + rclpy.spin_once(odometry_node) + pose = odometry_node.getPose3d() + return pose + except Exception as e: + print(f"Exception in hal getPose3d {repr(e)}") + + +# Bumper +def getBumperData(): + try: + rclpy.spin_once(bumper_node) + return bumper_node.getBumperData() + except Exception as e: + print(f"Exception in hal getBumper {repr(e)}") + + +### SETTERS ### + + +# Linear speed +def setV(v): + motor_node.sendV(v) + + +# Angular speed +def setW(w): + motor_node.sendW(w) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/exercise.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/exercise.py deleted file mode 100755 index 764ca0bc7..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/exercise.py +++ /dev/null @@ -1,415 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function - -from websocket_server import WebsocketServer -import time -import threading -import subprocess -import sys -from datetime import datetime -import re -import json -import importlib -import os - -from gui import GUI, ThreadGUI -from hal import HAL -from console import start_console, close_console - - -class Template: - # Initialize class variables - # self.ideal_cycle to run an execution for atleast 1 second - # self.process for the current running process - def __init__(self): - self.measure_thread = None - self.thread = None - self.reload = False - self.stop_brain = False - self.user_code = "" - - # Time variables - self.ideal_cycle = 80 - self.measured_cycle = 80 - self.iteration_counter = 0 - self.real_time_factor = 0 - self.frequency_message = {'brain': '', 'gui': '', 'rtf': ''} - - self.server = None - self.client = None - self.host = sys.argv[1] - - # Initialize the GUI, HAL and Console behind the scenes - self.hal = HAL() - self.gui = GUI(self.host, self.hal) - - ################ --- BRAIN --- ################ - - # The process function - def process_code(self, source_code): - - # Redirect the information to console - start_console() - - # Reference Environment for the exec() function - iterative_code, sequential_code = self.parse_code(source_code) - - # Whatever the code is, first step is to just stop! - self.hal.motors.sendV(0) - self.hal.motors.sendW(0) - - # print("The debug level is " + str(debug_level) - # print(sequential_code) - # print(iterative_code) - - # The Python exec function - # Run the sequential part - gui_module, hal_module = self.generate_modules() - reference_environment = {"GUI": gui_module, "HAL": hal_module} - exec(sequential_code, reference_environment) - - # Run the iterative part inside template - # and keep the check for flag - while not self.reload: - while (self.stop_brain): - if (self.reload): - break - time.sleep(0.1) - - start_time = datetime.now() - - # Execute the iterative portion - exec(iterative_code, reference_environment) - - # Template specifics to run! - finish_time = datetime.now() - dt = finish_time - start_time - ms = (dt.days * 24 * 60 * 60 + dt.seconds) * \ - 1000 + dt.microseconds / 1000.0 - - # Keep updating the iteration counter - if (iterative_code == ""): - self.iteration_counter = 0 - else: - self.iteration_counter = self.iteration_counter + 1 - - # The code should be run for atleast the target time step - # If it's less put to sleep - if (ms < self.ideal_cycle): - time.sleep((self.ideal_cycle - ms) / 1000.0) - - close_console() - print("Current Thread Joined!") - - # Function to generate the modules for use in ACE Editor - - def generate_modules(self): - # Define HAL module - hal_module = importlib.util.module_from_spec( - importlib.machinery.ModuleSpec("HAL", None)) - hal_module.HAL = importlib.util.module_from_spec( - importlib.machinery.ModuleSpec("HAL", None)) - hal_module.HAL.motors = importlib.util.module_from_spec( - importlib.machinery.ModuleSpec("motors", None)) - - # Add HAL functions - hal_module.HAL.getPose3d = self.hal.getPose3d - hal_module.HAL.setV = self.hal.setV - hal_module.HAL.setW = self.hal.setW - hal_module.HAL.laser = self.hal.laser - hal_module.HAL.getLaserData = self.hal.getLaserData - hal_module.HAL.getBumperData = self.hal.getBumperData - - # Define GUI module - gui_module = importlib.util.module_from_spec( - importlib.machinery.ModuleSpec("GUI", None)) - gui_module.GUI = importlib.util.module_from_spec( - importlib.machinery.ModuleSpec("GUI", None)) - - # Add GUI functions - gui_module.GUI.showNumpy = self.gui.showNumpy - gui_module.GUI.getMap = self.gui.getMap - - # Add GUI functions - # gui_module.GUI.showImage = self.gui.showImage - - # Adding modules to system - # Protip: The names should be different from - # other modules, otherwise some errors - sys.modules["HAL"] = hal_module - sys.modules["GUI"] = gui_module - - return gui_module, hal_module - - # Function to measure the frequency of iterations - def measure_frequency(self): - previous_time = datetime.now() - # An infinite loop - while True: - # Sleep for 2 seconds - time.sleep(2) - - # Measure the current time and subtract from the previous time to get real time interval - 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 - - # Get the time period - try: - # Division by zero - self.measured_cycle = ms / self.iteration_counter - except: - self.measured_cycle = 0 - - # Reset the counter - self.iteration_counter = 0 - - # Send to client - self.send_frequency_message() - - - ################ --- EXERCISE --- ################ - - # Function for saving - def save_code(self, source_code): - with open('code/academy.py', 'w') as code_file: - code_file.write(source_code) - - # Function for loading - def load_code(self): - with open('code/academy.py', 'r') as code_file: - source_code = code_file.read() - - return source_code - - # Function to parse the code - # A few assumptions: - # 1. The user always passes sequential and iterative codes - # 2. Only a single infinite loop - def parse_code(self, source_code): - # Check for save/load - if (source_code[:5] == "#save"): - source_code = source_code[5:] - self.save_code(source_code) - - return "", "" - - elif (source_code[:5] == "#load"): - source_code = source_code + self.load_code() - self.server.send_message(self.client, source_code) - - return "", "" - - else: - sequential_code, iterative_code = self.seperate_seq_iter( - source_code) - return iterative_code, sequential_code - - # Function to parse code according to the debugging level - def debug_parse(self, source_code, debug_level): - if (debug_level == 1): - # If debug level is 0, then all the GUI operations should not be called - source_code = re.sub(r'GUI\..*', '', source_code) - - return source_code - - # Function to seperate the iterative and sequential code - def seperate_seq_iter(self, source_code): - if source_code == "": - return "", "" - - # Search for an instance of while True - infinite_loop = re.search( - r'[^ ]while\s*\(\s*True\s*\)\s*:|[^ ]while\s*True\s*:|[^ ]while\s*1\s*:|[^ ]while\s*\(\s*1\s*\)\s*:', source_code) - - # Seperate the content inside while True and the other - # (Seperating the sequential and iterative part!) - try: - start_index = infinite_loop.start() - iterative_code = source_code[start_index:] - sequential_code = source_code[:start_index] - - # Remove while True: syntax from the code - # And remove the the 4 spaces indentation before each command - iterative_code = re.sub( - r'[^ ]while\s*\(\s*True\s*\)\s*:|[^ ]while\s*True\s*:|[^ ]while\s*1\s*:|[^ ]while\s*\(\s*1\s*\)\s*:', '', iterative_code) - # Add newlines to match line on bug report - extra_lines = sequential_code.count('\n') - while (extra_lines >= 0): - iterative_code = '\n' + iterative_code - extra_lines -= 1 - iterative_code = re.sub(r'^[ ]{4}', '', iterative_code, flags=re.M) - - except: - sequential_code = source_code - iterative_code = "" - - return sequential_code, iterative_code - - # Function to generate and send frequency messages - def send_frequency_message(self): - # This function generates and sends frequency measures of the brain and gui - brain_frequency = 0 - gui_frequency = 0 - try: - brain_frequency = round(1000 / self.measured_cycle, 1) - except ZeroDivisionError: - brain_frequency = 0 - - try: - gui_frequency = round(1000 / self.thread_gui.measured_cycle, 1) - except ZeroDivisionError: - gui_frequency = 0 - - self.frequency_message["brain"] = brain_frequency - self.frequency_message["gui"] = gui_frequency - self.frequency_message["rtf"] = self.real_time_factor - - message = "#freq" + json.dumps(self.frequency_message) - self.server.send_message(self.client, message) - - def send_ping_message(self): - self.server.send_message(self.client, "#ping") - - # Function to notify the front end that the code was received and sent to execution - def send_code_message(self): - self.server.send_message(self.client, "#exec") - - # Function to track the real time factor from Gazebo statistics - # https://stackoverflow.com/a/17698359 - # (For reference, Python3 solution specified in the same answer) - def track_stats(self): - args = ["gz", "stats", "-p"] - # Prints gz statistics. "-p": Output comma-separated values containing- - # real-time factor (percent), simtime (sec), realtime (sec), paused (T or F) - stats_process = subprocess.Popen(args, stdout=subprocess.PIPE) - # bufsize=1 enables line-bufferred mode (the input buffer is flushed - # automatically on newlines if you would write to process.stdin ) - 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") - - # Function to maintain thread execution - def execute_thread(self, source_code): - # Keep checking until the thread is alive - # The thread will die when the coming iteration reads the flag - if (self.thread != None): - while self.thread.is_alive(): - time.sleep(0.2) - - # Turn the flag down, the iteration has successfully stopped! - self.reload = False - # New thread execution - self.thread = threading.Thread( - target=self.process_code, args=[source_code]) - self.thread.start() - self.send_code_message() - print("New Thread Started!") - - # Function to read and set frequency from incoming message - def read_frequency_message(self, message): - frequency_message = json.loads(message) - - # Set brain frequency - frequency = float(frequency_message["brain"]) - self.ideal_cycle = 1000.0 / frequency - - # Set gui frequency - frequency = float(frequency_message["gui"]) - self.thread_gui.ideal_cycle = 1000.0 / frequency - - return - - # The websocket function - # Gets called when there is an incoming message from the client - def handle(self, client, server, message): - if (message[:5] == "#freq"): - frequency_message = message[5:] - self.read_frequency_message(frequency_message) - time.sleep(1) - self.send_frequency_message() - return - - elif (message[:5] == "#ping"): - time.sleep(1) - self.send_ping_message() - return - - elif (message[:5] == "#code"): - try: - # Once received turn the reload flag up and send it to execute_thread function - self.user_code = message[6:] - # print(repr(code)) - self.reload = True - self.stop_brain = True - self.execute_thread(self.user_code) - except: - pass - - elif (message[:5] == "#rest"): - try: - self.reload = True - self.stop_brain = True - self.execute_thread(self.user_code) - except: - pass - - elif (message[:5] == "#stop"): - self.stop_brain = True - - elif (message[:5] == "#play"): - self.stop_brain = False - - # Function that gets called when the server is connected - def connected(self, client, server): - self.client = client - # Start the GUI update thread - self.thread_gui = ThreadGUI(self.gui) - self.thread_gui.start() - - # Start the real time factor tracker thread - self.stats_thread = threading.Thread(target=self.track_stats) - self.stats_thread.start() - - # Start measure frequency - self.measure_thread = threading.Thread(target=self.measure_frequency) - self.measure_thread.start() - - # Initialize the ping message - # self.send_frequency_message() - - print(client, 'connected') - - # Function that gets called when the connected closes - def handle_close(self, client, server): - print(client, 'closed') - - def run_server(self): - self.server = WebsocketServer(port=1905, host=self.host) - self.server.set_fn_new_client(self.connected) - self.server.set_fn_client_left(self.handle_close) - self.server.set_fn_message_received(self.handle) - - home_dir = os.path.expanduser('~') - - logged = False - while not logged: - try: - f = open(f"{home_dir}/ws_code.log", "w") - f.write("websocket_code=ready") - f.close() - logged = True - except: - time.sleep(0.1) - - self.server.run_forever() - - -# Execute! -if __name__ == "__main__": - server = Template() - server.run_server() diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/gui.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/gui.py deleted file mode 100755 index ed5db8045..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/gui.py +++ /dev/null @@ -1,245 +0,0 @@ -import json -import cv2 -import numpy as np -import base64 -import threading -import time -from datetime import datetime -from websocket_server import WebsocketServer -import logging -import os -import rclpy -import matplotlib.pyplot as plt -from shared.image import SharedImage -from interfaces.pose3d import ListenerPose3d - -from map import Map - -# Graphical User Interface Class - -# Matrix colors -red = [0, 0, 255] -orange = [0, 165, 255] -yellow = [0, 255, 255] -green = [0, 255, 0] -blue = [255, 0, 0] -indigo = [130, 0, 75] -violet = [211, 0, 148] - -class GUI: - # Initialization function - # The actual initialization - def __init__(self, host, hal): - t = threading.Thread(target=self.run_server) - - self.payload = {'map': ''} - self.server = None - self.client = None - self.init_coords = (171, 63) - self.start_coords = (201, 85.5) - - self.host = host - - self.acknowledge = False - self.acknowledge_lock = threading.Lock() - - self.shared_image = SharedImage("guiimage") - - self.hal = hal - t.start() - - # Create the lap object - self.map = Map(self.hal.pose3d) - - # Explicit initialization function - # Class method, so user can call it without instantiation - @classmethod - def initGUI(cls, host, console): - # self.payload = {'map': ''} - new_instance = cls(host, console) - return new_instance - - # 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() - - # encode the image data to be sent to websocket - def payloadImage(self): - - 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 process_colors(self, image): - colored_image = np.zeros((image.shape[0], image.shape[1], 3), dtype=np.uint8) - - # Grayscale for values < 128 - mask = image < 128 - colored_image[mask] = image[mask][:, None] * 2 - - # Color lookup table - color_table = { - 128: red, - 129: orange, - 130: yellow, - 131: green, - 132: blue, - 133: indigo, - 134: violet - } - - for value, color in color_table.items(): - mask = image == value - colored_image[mask] = color - - return colored_image - - # Update the gui - def update_gui(self): - # Payload Map Message - pos_message = self.map.getRobotCoordinates() - if (pos_message == self.init_coords): - pos_message = self.start_coords - ang_message = self.map.getRobotAngle() - pos_message = str(pos_message + ang_message) - self.payload["map"] = pos_message - - payload = self.payloadImage() - self.payload["image"] = json.dumps(payload) - - message = "#gui" + json.dumps(self.payload) - self.server.send_message(self.client, message) - - # Function to read the message from websocket - # Gets called when there is an incoming message from the client - def get_message(self, client, server, message): - # Acknowledge Message for GUI Thread - if (message[:4] == "#ack"): - self.set_acknowledge(True) - - # load the image data - def showNumpy(self, image): - self.shared_image.add(self.process_colors(image)) - - def getMap(self, url): - return plt.imread(url) - - # Activate the server - def run_server(self): - self.server = WebsocketServer(port=2303, host=self.host) - self.server.set_fn_new_client(self.get_client) - self.server.set_fn_message_received(self.get_message) - - home_dir = os.path.expanduser('~') - - logged = False - while not logged: - try: - f = open(f"{home_dir}/ws_gui.log", "w") - f.write("websocket_gui=ready") - f.close() - logged = True - except: - time.sleep(0.1) - - self.server.run_forever() - - # Function to reset - def reset_gui(self): - self.map.reset() - - -# This class decouples the user thread -# and the GUI update thread -class ThreadGUI: - def __init__(self, gui): - self.gui = gui - - # Time variables - self.ideal_cycle = 80 - self.measured_cycle = 80 - self.iteration_counter = 0 - - # Function to start the execution of threads - def start(self): - self.measure_thread = threading.Thread(target=self.measure_thread) - self.thread = threading.Thread(target=self.run) - - self.measure_thread.start() - self.thread.start() - - print("GUI Thread Started!") - - # The measuring thread to measure frequency - def measure_thread(self): - while (self.gui.client == None): - pass - - previous_time = datetime.now() - while (True): - # Sleep for 2 seconds - time.sleep(2) - - # Measure the current time and subtract from previous time to get real time interval - 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 - - # Get the time period - try: - # Division by zero - self.measured_cycle = ms / self.iteration_counter - except: - self.measured_cycle = 0 - - # Reset the counter - self.iteration_counter = 0 - - # The main thread of execution - def run(self): - while (self.gui.client == None): - pass - - while (True): - start_time = datetime.now() - self.gui.update_gui() - acknowledge_message = self.gui.get_acknowledge() - - while (acknowledge_message == False): - acknowledge_message = self.gui.get_acknowledge() - - self.gui.set_acknowledge(False) - - finish_time = datetime.now() - self.iteration_counter = self.iteration_counter + 1 - - dt = finish_time - start_time - ms = (dt.days * 24 * 60 * 60 + dt.seconds) * \ - 1000 + dt.microseconds / 1000.0 - if (ms < self.ideal_cycle): - time.sleep((self.ideal_cycle-ms) / 1000.0) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/hal.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/hal.py deleted file mode 100755 index 70a97af00..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/hal.py +++ /dev/null @@ -1,58 +0,0 @@ -import rclpy -import sys -import cv2 -import threading -import time -from datetime import datetime - -from interfaces.motors import PublisherMotors -from interfaces.pose3d import ListenerPose3d -from interfaces.laser import ListenerLaser -from interfaces.bumper import ListenerBumper - -# Hardware Abstraction Layer -class HAL: - IMG_WIDTH = 320 - IMG_HEIGHT = 240 - - def __init__(self): - rclpy.init(args=sys.argv) - rclpy.create_node('HAL') - - self.motors = PublisherMotors("/cmd_vel", 4, 0.3) - self.pose3d = ListenerPose3d("/odom") - self.laser = ListenerLaser("/roombaROS/laser/scan") - self.bumper = ListenerBumper("/roombaROS/events/bumper","roombaROS") - - # Spin nodes so that subscription callbacks load topic data - # Bumper has to be spinned differently so that GetEntityState works - executor = rclpy.executors.MultiThreadedExecutor() - executor.add_node(self.pose3d) - executor.add_node(self.laser) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - - print("HAL-Nodes Thread Started") - - # Explicit initialization functions - # Class method, so user can call it without instantiation - @classmethod - def initRobot(cls): - new_instance = cls() - return new_instance - - def getBumperData(self): - self.bumper.spin_bumper_node() - return self.bumper.getBumperData() - - def getPose3d(self): - return self.pose3d.getPose3d() - - def getLaserData(self): - return self.laser.getLaserData() - - def setV(self, velocity): - self.motors.sendV(velocity) - - def setW(self, velocity): - self.motors.sendW(velocity) \ No newline at end of file diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/__init__.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/__init__.py deleted file mode 100755 index e69de29bb..000000000 diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/bumper.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/bumper.py deleted file mode 100755 index 1d9783e35..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/bumper.py +++ /dev/null @@ -1,166 +0,0 @@ -import sys -import math -import rclpy -from rclpy.node import Node -import threading -from math import asin, atan2, pi -from gazebo_msgs.msg import ContactsState -from gazebo_msgs.srv import GetEntityState - - -class BumperData (): - - def __init__(self): - - self.bumper = 0 # Indicates that bumper is - self.state = 0 # pressed or no (1,0) - self.timeStamp = 0 # Time stamp [s] - - - def __str__(self): - s = "BumperData: {\n bumper: " + str(self.bumper) + "\n state: " + str(self.state) - s = s + "\n timeStamp: " + str(self.timeStamp) + "\n}" - return s - -class ListenerBumper(Node): - ''' - ROS Bumper Subscriber. Bumper Client to Receive Contacts State from ROS nodes. - ''' - def __init__(self, topic, robot): - ''' - ListenerBumper Constructor. - - @param topic: ROS topic to subscribe - - @type topic: String - - ''' - super().__init__("bumper_subscriber_node") - self.topic = topic - self.data = BumperData() - self.sub = None - self.lock = threading.Lock() - self.start() - - self.client = self.create_client(GetEntityState, '/gazebo/get_entity_state') - while not self.client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - self.req = GetEntityState.Request() - self.req.name = robot - self.future = None - self.future_lock = threading.Lock() - - def spin_bumper_node(self): - - future = self.client.call_async(self.req) - rclpy.spin_until_future_complete(self, future) - - self.future_lock.acquire() - self.future = future - self.future_lock.release() - - def __callback (self, event): - ''' - Callback function to receive and save Contacts State. - - @param event: ROS ContactsState received - - @type event: ContactsState - - ''' - bump = self.contactsState2BumperData(event) - - self.lock.acquire() - self.data = bump - self.lock.release() - - - def stop(self): - ''' - Stops (Unregisters) the client. - - ''' - self.sub.unregister() - - def start (self): - ''' - Starts (Subscribes) the client. - - ''' - self.sub = self.create_subscription(ContactsState, self.topic , self.__callback,10) - - def getBumperData(self): - ''' - Returns last BumperData. - - @return last JdeRobotTypes BumperData saved - - ''' - self.lock.acquire() - bump = self.data - self.lock.release() - - return bump - - def contactsState2BumperData(self,event): - ''' - Translates from ROS ContactsState to JderobotTypes BumperData. - - @param event: ROS ContactsState to translate - - @type event: ContactsState - - @return a BumperData translated from event - - # bumper - LEFT = 0 - CENTER = 1 - RIGHT = 2 - - # state - RELEASED = 0 - PRESSED = 1 - - ''' - bump = BumperData() - if len(event.states) > 0: - contact_x = event.states[0].contact_normals[0].x - contact_y = event.states[0].contact_normals[0].y - - self.future_lock.acquire() - robot_coordinates = self.future.result() - self.future_lock.release() - - if robot_coordinates is not None: - - qx = robot_coordinates.state.pose.orientation.x - qy = robot_coordinates.state.pose.orientation.y - qz = robot_coordinates.state.pose.orientation.z - qw = robot_coordinates.state.pose.orientation.w - - rotateZa0=2.0*(qx*qy + qw*qz) - rotateZa1=qw*qw + qx*qx - qy*qy - qz*qz - robotYaw=0.0 - if(rotateZa0 != 0.0 and rotateZa1 != 0.0): - robotYaw=math.atan2(rotateZa0,rotateZa1) - - global_contact_angle = 0.0 - if(contact_x != 0.0 and contact_y != 0.0): - global_contact_angle = math.atan2(contact_y,contact_x) - - relative_contact_angle = global_contact_angle - robotYaw -math.pi - - bump.state = 1 - if relative_contact_angle <= math.pi/2 and relative_contact_angle > math.pi/6: #0.52, 1.5 - bump.bumper = 0 - elif relative_contact_angle <= math.pi/6 and relative_contact_angle > -math.pi/6: # -0.52, 0.52 - bump.bumper = 1 - elif relative_contact_angle < -math.pi/6 and relative_contact_angle >= -math.pi/2: #-1.5 , -0.52 - bump.bumper = 2 - else: - bump.state = 0 - else: - bump.state = 0 - - bump.timeStamp = event.header.stamp.sec + (event.header.stamp.nanosec *1e-9) - return bump \ No newline at end of file diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/laser.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/laser.py deleted file mode 100755 index dc2ce601d..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/laser.py +++ /dev/null @@ -1,116 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import LaserScan -import threading -from math import pi as PI - -class LaserData (): - - def __init__(self): - - self.values = [] # meters - self.minAngle = 0 # Angle of first value (rads) - self.maxAngle = 0 # Angle of last value (rads) - self.minRange = 0 # Max Range posible (meters) - self.maxRange = 0 #Min Range posible (meters) - self.timeStamp = 0 # seconds - - - def __str__(self): - s = "LaserData: {\n minAngle: " + str(self.minAngle) + "\n maxAngle: " + str(self.maxAngle) - s = s + "\n minRange: " + str(self.minRange) + "\n maxRange: " + str(self.maxRange) - s = s + "\n timeStamp: " + str(self.timeStamp) + "\n values: " + str(self.values) + "\n}" - return s - - -def laserScan2LaserData(scan): - ''' - Translates from ROS LaserScan to JderobotTypes LaserData. - - @param scan: ROS LaserScan to translate - - @type scan: LaserScan - - @return a LaserData translated from scan - - ''' - laser = LaserData() - laser.values = scan.ranges - ''' - ROS Angle Map JdeRobot Angle Map - 0 PI/2 - | | - | | - PI/2 --------- -PI/2 PI --------- 0 - | | - | | - ''' - laser.minAngle = scan.angle_min + PI/2 - laser.maxAngle = scan.angle_max + PI/2 - laser.maxRange = scan.range_max - laser.minRange = scan.range_min - laser.timeStamp = scan.header.stamp.sec + (scan.header.stamp.nanosec *1e-9) - return laser - -class ListenerLaser(Node): - ''' - ROS Laser Subscriber. Laser Client to Receive Laser Scans from ROS nodes. - ''' - def __init__(self, topic): - ''' - ListenerLaser Constructor. - - @param topic: ROS topic to subscribe - - @type topic: String - - ''' - super().__init__("laser_subscriber_node") - self.topic = topic - self.data = LaserData() - self.sub = None - self.lock = threading.Lock() - self.start() - - def __callback (self, scan): - ''' - Callback function to receive and save Laser Scans. - - @param scan: ROS LaserScan received - - @type scan: LaserScan - - ''' - laser = laserScan2LaserData(scan) - - self.lock.acquire() - self.data = laser - self.lock.release() - - def stop(self): - ''' - Stops (Unregisters) the client. - - ''' - self.sub.unregister() - - def start (self): - ''' - Starts (Subscribes) the client. - - ''' - self.sub = self.create_subscription(LaserScan, self.topic , self.__callback,10) - - def getLaserData(self): - ''' - Returns last LaserData. - - @return last JdeRobotTypes LaserData saved - - ''' - self.lock.acquire() - laser = self.data - self.lock.release() - - return laser - diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/motors.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/motors.py deleted file mode 100755 index a889adff1..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/motors.py +++ /dev/null @@ -1,121 +0,0 @@ -import rclpy -from geometry_msgs.msg import Twist -import threading -from math import pi as PI -from .threadPublisher import ThreadPublisher - -def cmdvel2Twist(vel): - - tw = Twist() - tw.linear.x = float(vel.vx) - tw.linear.y = float(vel.vy) - tw.linear.z = float(vel.vz) - tw.angular.x = float(vel.ax) - tw.angular.y = float(vel.ay) - tw.angular.z = float(vel.az) - - return tw - - -class CMDVel (): - - def __init__(self): - - self.vx = 0 # vel in x[m/s] (use this for V in wheeled robots) - self.vy = 0 # vel in y[m/s] - self.vz = 0 # vel in z[m/s] - self.ax = 0 # angular vel in X axis [rad/s] - self.ay = 0 # angular vel in X axis [rad/s] - self.az = 0 # angular vel in Z axis [rad/s] (use this for W in wheeled robots) - self.timeStamp = 0 # Time stamp [s] - - - def __str__(self): - s = "CMDVel: {\n vx: " + str(self.vx) + "\n vy: " + str(self.vy) - s = s + "\n vz: " + str(self.vz) + "\n ax: " + str(self.ax) - s = s + "\n ay: " + str(self.ay) + "\n az: " + str(self.az) - s = s + "\n timeStamp: " + str(self.timeStamp) + "\n}" - return s - -class PublisherMotors: - - def __init__(self, topic, maxV, maxW): - - self.maxW = maxW - self.maxV = maxV - - self.node = rclpy.create_node('PublisherMotors') - self.topic = topic - self.data = CMDVel() - self.pub = self.node.create_publisher(Twist, self.topic, 10 ) - - self.lock = threading.Lock() - - self.kill_event = threading.Event() - self.thread = ThreadPublisher(self, self.kill_event) - - self.thread.daemon = True - self.start() - - def publish (self): - - - self.lock.acquire() - tw = cmdvel2Twist(self.data) - self.lock.release() - self.pub.publish(tw) - - def stop(self): - - self.kill_event.set() - self.pub.unregister() - - def start (self): - - self.kill_event.clear() - self.thread.start() - - - def getMaxW(self): - return self.maxW - - def getMaxV(self): - return self.maxV - - - def sendVelocities(self, vel): - - self.lock.acquire() - self.data = vel - self.lock.release() - - def sendV(self, v): - self.sendVX(v) - - def sendL(self, l): - - self.sendVY(l) - - def sendW(self, w): - - self.sendAZ(w) - - def sendVX(self, vx): - - self.lock.acquire() - self.data.vx = float(vx) - self.lock.release() - - def sendVY(self, vy): - - self.lock.acquire() - self.data.vy = float(vy) - self.lock.release() - - def sendAZ(self, az): - - self.lock.acquire() - self.data.az = float(az) - self.lock.release() - - diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/pose3d.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/pose3d.py deleted file mode 100755 index 9ab4cd412..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/pose3d.py +++ /dev/null @@ -1,179 +0,0 @@ -import rclpy -from rclpy.node import Node -import threading -from math import asin, atan2, pi -from nav_msgs.msg import Odometry - -def quat2Yaw(qw, qx, qy, qz): - ''' - Translates from Quaternion to Yaw. - - @param qw,qx,qy,qz: Quaternion values - - @type qw,qx,qy,qz: float - - @return Yaw value translated from Quaternion - - ''' - rotateZa0=2.0*(qx*qy + qw*qz) - rotateZa1=qw*qw + qx*qx - qy*qy - qz*qz - rotateZ=0.0 - if(rotateZa0 != 0.0 and rotateZa1 != 0.0): - rotateZ=atan2(rotateZa0,rotateZa1) - return rotateZ - -def quat2Pitch(qw, qx, qy, qz): - ''' - Translates from Quaternion to Pitch. - - @param qw,qx,qy,qz: Quaternion values - - @type qw,qx,qy,qz: float - - @return Pitch value translated from Quaternion - - ''' - - rotateYa0=-2.0*(qx*qz - qw*qy) - rotateY=0.0 - if(rotateYa0 >= 1.0): - rotateY = pi/2.0 - elif(rotateYa0 <= -1.0): - rotateY = -pi/2.0 - else: - rotateY = asin(rotateYa0) - - return rotateY - -def quat2Roll (qw, qx, qy, qz): - ''' - Translates from Quaternion to Roll. - - @param qw,qx,qy,qz: Quaternion values - - @type qw,qx,qy,qz: float - - @return Roll value translated from Quaternion - - ''' - rotateXa0=2.0*(qy*qz + qw*qx) - rotateXa1=qw*qw - qx*qx - qy*qy + qz*qz - rotateX=0.0 - - if(rotateXa0 != 0.0 and rotateXa1 != 0.0): - rotateX=atan2(rotateXa0, rotateXa1) - return rotateX - - -def odometry2Pose3D(odom): - ''' - Translates from ROS Odometry to JderobotTypes Pose3d. - - @param odom: ROS Odometry to translate - - @type odom: Odometry - - @return a Pose3d translated from odom - - ''' - pose = Pose3d() - ori = odom.pose.pose.orientation - - pose.x = odom.pose.pose.position.x - pose.y = odom.pose.pose.position.y - pose.z = odom.pose.pose.position.z - #pose.h = odom.pose.pose.position.h - pose.yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z) - pose.pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z) - pose.roll = quat2Roll(ori.w, ori.x, ori.y, ori.z) - pose.q = [ori.w, ori.x, ori.y, ori.z] - pose.timeStamp = odom.header.stamp.sec + (odom.header.stamp.nanosec *1e-9) - - return pose - -class Pose3d (): - - def __init__(self): - - self.x = 0 # X coord [meters] - self.y = 0 # Y coord [meters] - self.z = 0 # Z coord [meters] - self.h = 1 # H param - self.yaw = 0 #Yaw angle[rads] - self.pitch = 0 # Pitch angle[rads] - self.roll = 0 # Roll angle[rads] - self.q = [0,0,0,0] # Quaternion - self.timeStamp = 0 # Time stamp [s] - - - def __str__(self): - s = "Pose3D: {\n x: " + str(self.x) + "\n Y: " + str(self.y) - s = s + "\n Z: " + str(self.z) + "\n H: " + str(self.h) - s = s + "\n Yaw: " + str(self.yaw) + "\n Pitch: " + str(self.pitch) + "\n Roll: " + str(self.roll) - s = s + "\n quaternion: " + str(self.q) + "\n timeStamp: " + str(self.timeStamp) + "\n}" - return s - - -class ListenerPose3d(Node): - ''' - ROS Pose3D Subscriber. Pose3D Client to Receive pose3d from ROS nodes. - ''' - def __init__(self, topic): - ''' - ListenerPose3d Constructor. - - @param topic: ROS topic to subscribe - - @type topic: String - - ''' - super().__init__("ListenerPose") - self.topic = topic - self.data = Pose3d() - self.sub = None - self.lock = threading.Lock() - self.start() - - def __callback (self, odom): - ''' - Callback function to receive and save Pose3d. - - @param odom: ROS Odometry received - - @type odom: Odometry - - ''' - pose = odometry2Pose3D(odom) - - self.lock.acquire() - self.data = pose - self.lock.release() - - def stop(self): - ''' - Stops (Unregisters) the client. - - ''' - self.sub.unregister() - - def start (self): - ''' - Starts (Subscribes) the client. - - ''' - self.sub = self.create_subscription(Odometry, self.topic, self.__callback,10) - - - def getPose3d(self): - ''' - Returns last Pose3d. - - @return last JdeRobotTypes Pose3d saved - - ''' - self.lock.acquire() - pose = self.data - self.lock.release() - - return pose - diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/threadPublisher.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/threadPublisher.py deleted file mode 100755 index 69aa0ad48..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/interfaces/threadPublisher.py +++ /dev/null @@ -1,46 +0,0 @@ -# -# Copyright (C) 1997-2016 JDE Developers Team -# -# This program is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program. If not, see http://www.gnu.org/licenses/. -# Authors : -# Alberto Martin Florido -# Aitor Martinez Fernandez -# -import threading -import time -from datetime import datetime - -time_cycle = 80 - - -class ThreadPublisher(threading.Thread): - - def __init__(self, pub, kill_event): - self.pub = pub - self.kill_event = kill_event - threading.Thread.__init__(self, args=kill_event) - - def run(self): - while (not self.kill_event.is_set()): - start_time = datetime.now() - - self.pub.publish() - - finish_Time = datetime.now() - - dt = finish_Time - start_time - ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 - #print (ms) - if (ms < time_cycle): - time.sleep((time_cycle - ms) / 1000.0) \ No newline at end of file diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py index dec52ee53..cf05aaf6b 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py @@ -8,18 +8,36 @@ def __init__(self, pose3d): self.pose3d = pose3d def RTx(self, angle, tx, ty, tz): - RT = np.matrix([[1, 0, 0, tx], [0, math.cos(angle), -math.sin(angle), ty], - [0, math.sin(angle), math.cos(angle), tz], [0, 0, 0, 1]]) + RT = np.matrix( + [ + [1, 0, 0, tx], + [0, math.cos(angle), -math.sin(angle), ty], + [0, math.sin(angle), math.cos(angle), tz], + [0, 0, 0, 1] + ] + ) return RT def RTy(self, angle, tx, ty, tz): - RT = np.matrix([[math.cos(angle), 0, math.sin(angle), tx], [0, 1, 0, ty], - [-math.sin(angle), 0, math.cos(angle), tz], [0, 0, 0, 1]]) + RT = np.matrix( + [ + [math.cos(angle), 0, math.sin(angle), tx], + [0, 1, 0, ty], + [-math.sin(angle), 0, math.cos(angle), tz], + [0, 0, 0, 1] + ] + ) return RT def RTz(self, angle, tx, ty, tz): - RT = np.matrix([[math.cos(angle), -math.sin(angle), 0, tx], [math.sin(angle), math.cos(angle), 0, ty], - [0, 0, 1, tz], [0, 0, 0, 1]]) + RT = np.matrix( + [ + [math.cos(angle), -math.sin(angle), 0, tx], + [math.sin(angle), math.cos(angle), 0, ty], + [0, 0, 1, tz], + [0, 0, 0, 1] + ] + ) return RT def RTVacuum(self): From 36f3384529dc0b83f9ef359d24afdee5f32293e2 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Fri, 24 May 2024 13:17:35 +0200 Subject: [PATCH 02/12] Updating gui, hal and map --- .../python_template/ros2_humble/GUI.py | 3 +- .../python_template/ros2_humble/HAL.py | 4 +- .../python_template/ros2_humble/map.py | 128 +++++++++--------- 3 files changed, 69 insertions(+), 66 deletions(-) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py index e970251dc..07fa5738c 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py @@ -4,6 +4,7 @@ import threading import time import websocket +import numpy as np from src.manager.ram_logging.log_manager import LogManager from gazebo_msgs.srv import SetEntityState, GetEntityState from shared.image import SharedImage @@ -186,7 +187,7 @@ def getMap(self, url): gui = ThreadingGUI(host) def showNumpy(image): - gui.show(image) + gui.showNumpy(image) # Redirect the console start_console() \ No newline at end of file diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py index 9aebdc5ba..982d32f98 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py @@ -61,9 +61,9 @@ def getBumperData(): # Linear speed def setV(v): - motor_node.sendV(v) + motor_node.sendV(float(v)) # Angular speed def setW(w): - motor_node.sendW(w) + motor_node.sendW(float(w)) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py index cf05aaf6b..28862822d 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py @@ -3,71 +3,73 @@ from math import pi as pi import cv2 + class Map: - def __init__(self, pose3d): - self.pose3d = pose3d - - def RTx(self, angle, tx, ty, tz): - RT = np.matrix( - [ - [1, 0, 0, tx], - [0, math.cos(angle), -math.sin(angle), ty], - [0, math.sin(angle), math.cos(angle), tz], - [0, 0, 0, 1] - ] - ) - return RT - - def RTy(self, angle, tx, ty, tz): - RT = np.matrix( - [ - [math.cos(angle), 0, math.sin(angle), tx], - [0, 1, 0, ty], - [-math.sin(angle), 0, math.cos(angle), tz], - [0, 0, 0, 1] - ] - ) - return RT - - def RTz(self, angle, tx, ty, tz): - RT = np.matrix( - [ - [math.cos(angle), -math.sin(angle), 0, tx], - [math.sin(angle), math.cos(angle), 0, ty], - [0, 0, 1, tz], - [0, 0, 0, 1] - ] - ) - return RT - - def RTVacuum(self): - RTz = self.RTz(pi/2, 50, 70, 0) - return RTz - - def getRobotCoordinates(self): - pose = self.pose3d.getPose3d() - x = pose.x - y = pose.y - - scale_y = 15; offset_y = 63 - y = scale_y * y + offset_y - - scale_x = -30; offset_x = 171 - x = scale_x * x + offset_x - - return x, y + def __init__(self, pose_getter): + self.pose_getter = pose_getter + + def RTx(self, angle, tx, ty, tz): + RT = np.matrix( + [ + [1, 0, 0, tx], + [0, math.cos(angle), -math.sin(angle), ty], + [0, math.sin(angle), math.cos(angle), tz], + [0, 0, 0, 1], + ] + ) + return RT + + def RTy(self, angle, tx, ty, tz): + RT = np.matrix( + [ + [math.cos(angle), 0, math.sin(angle), tx], + [0, 1, 0, ty], + [-math.sin(angle), 0, math.cos(angle), tz], + [0, 0, 0, 1], + ] + ) + return RT + + def RTz(self, angle, tx, ty, tz): + RT = np.matrix( + [ + [math.cos(angle), -math.sin(angle), 0, tx], + [math.sin(angle), math.cos(angle), 0, ty], + [0, 0, 1, tz], + [0, 0, 0, 1], + ] + ) + return RT + + def RTVacuum(self): + RTz = self.RTz(pi / 2, 50, 70, 0) + return RTz + + def getRobotCoordinates(self): + pose = self.pose_getter() + x = pose.x + y = pose.y + + scale_y = 15 + offset_y = 63 + y = scale_y * y + offset_y + + scale_x = -30 + offset_x = 171 + x = scale_x * x + offset_x + + return x, y - def getRobotAngle(self): - pose = self.pose3d.getPose3d() - rt = pose.yaw + def getRobotAngle(self): + pose = self.pose_getter() + rt = pose.yaw - ty = math.cos(-rt) - math.sin(-rt) - tx = math.sin(-rt) + math.cos(-rt) + ty = math.cos(-rt) - math.sin(-rt) + tx = math.sin(-rt) + math.cos(-rt) - return tx, ty + return tx, ty - # Function to reset - def reset(self): - # Nothing to do, service takes care! - pass - + # Function to reset + def reset(self): + # Nothing to do, service takes care! + pass From ae4bb0d728358576d80e566d1b44715a392eb150 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Fri, 24 May 2024 17:09:14 +0200 Subject: [PATCH 03/12] Missing map update --- .../python_template/ros2_humble/GUI.py | 82 +++++------------- .../python_template/ros2_humble/HAL.py | 57 ++++++++---- .../python_template/ros2_humble/map.py | 6 ++ .../react-components/SpecificVacuumCleaner.js | 35 ++++---- .../resources/{ => images}/mapgrannyannie.png | Bin 5 files changed, 82 insertions(+), 98 deletions(-) rename exercises/static/exercises/vacuum_cleaner_loc_newmanager/resources/{ => images}/mapgrannyannie.png (100%) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py index 07fa5738c..c6a7e9abb 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py @@ -1,21 +1,19 @@ import json import cv2 import base64 +import numpy as np +import matplotlib.pyplot as plt import threading import time import websocket -import numpy as np from src.manager.ram_logging.log_manager import LogManager from gazebo_msgs.srv import SetEntityState, GetEntityState from shared.image import SharedImage -import matplotlib.pyplot as plt import rclpy from console import start_console from map import Map from HAL import getPose3d -from map import Map - # Graphical User Interface Class # Matrix colors @@ -44,8 +42,7 @@ def __init__(self, host="ws://127.0.0.1:2303", freq=30.0): self.node = rclpy.create_node("node") # Payload vars - # self.msg = {"pos_msg": "", "ang_msg": ""} - self.msg = {'map': '', 'nav': ''} + self.msg = {"pos_msg": "", "ang_msg": ""} self.init_coords = (171, 63) self.start_coords = (201, 85.5) self.map = Map(getPose3d) @@ -82,7 +79,7 @@ def gui_out_thread(self): # Check if a new map should be sent with self.ack_lock: if self.ack and self.map is not None: - self.update_gui() + self.send_map() self.ack = False # Maintain desired frequency @@ -90,67 +87,24 @@ def gui_out_thread(self): sleep_time = max(0, self.out_period - elapsed) time.sleep(sleep_time) - # Update the gui - def update_gui(self): - # Payload Map Message + # Prepares and sends a map to the websocket server + def send_map(self): + + # Get the necessary info pos_message = self.map.getRobotCoordinates() - if (pos_message == self.init_coords): + if pos_message == self.init_coords: pos_message = self.start_coords ang_message = self.map.getRobotAngle() - pos_message = str(pos_message + ang_message) - self.msg["map"] = pos_message - - # Example Payload Navigation Data message (random data) - # 4 colors supported (0, 1, 2, 3) - #nav_mat = np.zeros((20, 20), int) - #nav_mat[2, 1] = 1 - #nav_mat[3, 3] = 2 - #nav_mat[5,9] = 3 - #nav_message = str(nav_mat.tolist()) - payload = self.payloadImage() - self.msg["image"] = json.dumps(payload) + self.msg["pos_msg"] = pos_message + self.msg["ang_msg"] = ang_message message = json.dumps(self.msg) - if self.client: - try: + try: + if self.client: self.client.send(message) - except Exception as e: - print(f"Error sending message: {e}") - + except Exception as e: + LogManager.logger.info(f"Error sending message: {e}") - # Prepares and sends a map to the websocket server - # def send_map(self): - - # # Get the necessary info - # pos_message = self.map.getRobotCoordinates() - # if pos_message == self.init_coords: - # pos_message = self.start_coords - # ang_message = self.map.getRobotAngle() - - # self.msg["pos_msg"] = pos_message - # self.msg["ang_msg"] = ang_message - # 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}") - - # encode the image data to be sent to websocket - def payloadImage(self): - - 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 process_colors(self, image): colored_image = np.zeros((image.shape[0], image.shape[1], 3), dtype=np.uint8) @@ -186,8 +140,12 @@ def getMap(self, url): host = "ws://127.0.0.1:2303" gui = ThreadingGUI(host) +# Expose to the user def showNumpy(image): gui.showNumpy(image) +def getMap(self, url): + return gui.image(url) + # Redirect the console -start_console() \ No newline at end of file +start_console() diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py index 982d32f98..2b3689b5c 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py @@ -1,5 +1,7 @@ import rclpy import sys +import threading + from console import start_console from hal_interfaces.general.motors import MotorsNode @@ -14,7 +16,7 @@ rclpy.init(args=sys.argv) motor_node = MotorsNode("/cmd_vel", 4, 0.3) -odometry_node = OdometryNode("/odom") # TODO: check if correct or "/gazebo/model_states" +odometry_node = OdometryNode("/odom") laser_node = LaserNode("/roombaROS/laser/scan") bumper_node = BumperNode( [ @@ -24,27 +26,48 @@ ] ) -### GETTERS ### +# Spin nodes so that subscription callbacks load topic data +executor = rclpy.executors.MultiThreadedExecutor() +executor.add_node(odometry_node) +executor.add_node(laser_node) +executor_thread = threading.Thread(target=executor.spin, daemon=True) +executor_thread.start() + +print("HAL-Nodes Thread Started") +def getPose3d(): + return odometry_node.getPose3d() -# Laser def getLaserData(): - try: - rclpy.spin_once(laser_node) - values = laser_node.getLaserData().values - return values - except Exception as e: - print(f"Exception in hal getLaserData {repr(e)}") + laser_data = laser_node.getLaserData() + while len(laser_data.values) == 0: + laser_data = laser_node.getLaserData() + return laser_data +### GETTERS ### -# Pose -def getPose3d(): - try: - rclpy.spin_once(odometry_node) - pose = odometry_node.getPose3d() - return pose - except Exception as e: - print(f"Exception in hal getPose3d {repr(e)}") + +# # Laser +# def getLaserData(): +# try: +# rclpy.spin_once(laser_node) +# values = laser_node.getLaserData().values +# return values +# except Exception as e: +# print(f"Exception in hal getLaserData {repr(e)}") + + +# # Pose +# def getPose3d(): +# try: +# rclpy.spin_once(odometry_node) +# pose = odometry_node.getPose3d() +# while pose == None: +# pose = odometry_node.getPose3d() +# print(pose) +# return pose +# except Exception as e: +# print(f"Exception in hal getPose3d {repr(e)}") # Bumper diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py index 28862822d..e89fc99e9 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py @@ -47,6 +47,9 @@ def RTVacuum(self): def getRobotCoordinates(self): pose = self.pose_getter() + while pose != None: + pose = self.pose_getter() + print(pose) x = pose.x y = pose.y @@ -62,6 +65,9 @@ def getRobotCoordinates(self): def getRobotAngle(self): pose = self.pose_getter() + while pose != None: + pose = self.pose_getter() + print(pose) rt = pose.yaw ty = math.cos(-rt) - math.sin(-rt) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js index 008eeac21..bd15d7dd2 100644 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js @@ -2,28 +2,27 @@ import * as React from "react"; import PropTypes from "prop-types"; import { draw } from "Helpers/BirdEye"; -function SpecificVacuumCleaner(props) { +export default function SpecificVacuumCleaner() { const guiCanvasRef = React.useRef(); React.useEffect(() => { - console.log("SpecificVacuumCleaner subscribing to ['update'] events"); + console.log("TestShowScreen subscribing to ['update'] events"); const callback = (message) => { console.log(message); - const data = message.data.update; - const pos_msg = data.pos_msg; - const ang_msg = data.ang_msg; - - draw( - guiCanvasRef.current, - pos_msg[0], - pos_msg[1], - ang_msg[0], - ang_msg[1] - ); - - // Send the ACK of the img - window.RoboticsExerciseComponents.commsManager.send("gui", "ack"); + if (data.map) { + const pose = data.map.substring(1, data.map.length - 1); + const content = pose.split(",").map(function (item) { + return parseFloat(item); + }); + draw( + guiCanvasRef.current, + content[0], + content[1], + content[2], + content[3] + ); + } }; window.RoboticsExerciseComponents.commsManager.subscribe( @@ -45,7 +44,7 @@ function SpecificVacuumCleaner(props) { ref={guiCanvasRef} style={{ backgroundImage: - "url('/static/exercises/vacuum_cleaner_newmanager/resources/images/mapgrannyannie.png')", + "url('/static/exercises/vacuum_cleaner_loc_newmanager/resources/images/mapgrannyannie.png')", border: "2px solid #d3d3d3", backgroundRepeat: "no-repeat", backgroundSize: "100% 100%", @@ -59,5 +58,3 @@ function SpecificVacuumCleaner(props) { SpecificVacuumCleaner.propTypes = { circuit: PropTypes.string, }; - -export default SpecificVacuumCleaner; diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/resources/mapgrannyannie.png b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/resources/images/mapgrannyannie.png similarity index 100% rename from exercises/static/exercises/vacuum_cleaner_loc_newmanager/resources/mapgrannyannie.png rename to exercises/static/exercises/vacuum_cleaner_loc_newmanager/resources/images/mapgrannyannie.png From 3b43c0fbc2cea55fd5096a33699645afb7cd2e7f Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Fri, 24 May 2024 17:27:46 +0200 Subject: [PATCH 04/12] Updating GUI --- .../python_template/ros2_humble/GUI.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py index c6a7e9abb..b5ae58e60 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py @@ -7,7 +7,6 @@ import time import websocket from src.manager.ram_logging.log_manager import LogManager -from gazebo_msgs.srv import SetEntityState, GetEntityState from shared.image import SharedImage import rclpy from console import start_console @@ -55,7 +54,7 @@ def __init__(self, host="ws://127.0.0.1:2303", freq=30.0): # Initialize and start the image sending thread (GUI out thread) threading.Thread( - target=self.gui_out_thread, name="gui_out_thread", daemon=True + target=self.gui_out_thread, name="gui_out_thread_loc", daemon=True ).start() # Init websocket client From 78e5402aaf2add238fc60edf1827d6aba15b31e0 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Fri, 24 May 2024 17:54:00 +0200 Subject: [PATCH 05/12] Reverting .js --- .../react-components/SpecificVacuumCleaner.js | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js index bd15d7dd2..3104c37a5 100644 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js @@ -10,19 +10,19 @@ export default function SpecificVacuumCleaner() { const callback = (message) => { console.log(message); - if (data.map) { - const pose = data.map.substring(1, data.map.length - 1); - const content = pose.split(",").map(function (item) { - return parseFloat(item); - }); - draw( - guiCanvasRef.current, - content[0], - content[1], - content[2], - content[3] - ); - } + // if (data.map) { + // const pose = data.map.substring(1, data.map.length - 1); + // const content = pose.split(",").map(function (item) { + // return parseFloat(item); + // }); + // draw( + // guiCanvasRef.current, + // content[0], + // content[1], + // content[2], + // content[3] + // ); + // } }; window.RoboticsExerciseComponents.commsManager.subscribe( From cc48afdb0308df7d797ec39666b1c88af78e412b Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Sun, 26 May 2024 20:34:01 +0200 Subject: [PATCH 06/12] HAL working --- .../python_template/ros2_humble/HAL.py | 55 ++++++------------- 1 file changed, 16 insertions(+), 39 deletions(-) diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py index 2b3689b5c..5900922fd 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py @@ -1,7 +1,5 @@ import rclpy import sys -import threading - from console import start_console from hal_interfaces.general.motors import MotorsNode @@ -26,48 +24,27 @@ ] ) -# Spin nodes so that subscription callbacks load topic data -executor = rclpy.executors.MultiThreadedExecutor() -executor.add_node(odometry_node) -executor.add_node(laser_node) -executor_thread = threading.Thread(target=executor.spin, daemon=True) -executor_thread.start() - -print("HAL-Nodes Thread Started") +### GETTERS ### -def getPose3d(): - return odometry_node.getPose3d() +# Laser def getLaserData(): - laser_data = laser_node.getLaserData() - while len(laser_data.values) == 0: - laser_data = laser_node.getLaserData() - return laser_data - -### GETTERS ### + try: + rclpy.spin_once(laser_node) + values = laser_node.getLaserData().values + return values + except Exception as e: + print(f"Exception in hal getLaserData {repr(e)}") -# # Laser -# def getLaserData(): -# try: -# rclpy.spin_once(laser_node) -# values = laser_node.getLaserData().values -# return values -# except Exception as e: -# print(f"Exception in hal getLaserData {repr(e)}") - - -# # Pose -# def getPose3d(): -# try: -# rclpy.spin_once(odometry_node) -# pose = odometry_node.getPose3d() -# while pose == None: -# pose = odometry_node.getPose3d() -# print(pose) -# return pose -# except Exception as e: -# print(f"Exception in hal getPose3d {repr(e)}") +# Pose +def getPose3d(): + try: + rclpy.spin_once(odometry_node) + pose = odometry_node.getPose3d() + return pose + except Exception as e: + print(f"Exception in hal getPose3d {repr(e)}") # Bumper From 311443ae623dfaf03604007acdba5877bc048300 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Sun, 26 May 2024 20:58:02 +0200 Subject: [PATCH 07/12] Working, but no Reset --- db.sqlite3 | Bin 327680 -> 327680 bytes .../python_template/ros2_humble/GUI.py | 214 +++++++++++------- .../python_template/ros2_humble/map.py | 124 ++++------ .../react-components/SpecificVacuumCleaner.js | 57 +++-- 4 files changed, 229 insertions(+), 166 deletions(-) diff --git a/db.sqlite3 b/db.sqlite3 index f7dc38f9e40ca5e1493159e0766db874f4b99136..dab8ac8274670bb87d1fe6a9875ba22654450a27 100644 GIT binary patch delta 1341 zcmah}Z)h837|;7Bd2{WxNz<-fm$td2t~j^e?k;ziyNuChv(v7&#g^I0{W#XI=@ zcwX*#e$PETzqeSdDHdzq_Jh0cH0%X;FAx3BZ?4^hP%BIvi>JpjgYm@ptLed1W(+pZ z9o;$7?vo@=RHL#U6*OKChc#K9Ud$Ma8L+d6l8B{b6)Zx;&Skwkq}v~YvU;^uP}b$h zvK O(+KF^#^#1(fJU3{sj`rC4+nd%OOja$xZSdF(Yz``mVrzF0w?vA=l=DLYuSA zg#aY5*`|<9!8LM2&ZdBU*kn_qeb`_}e&Y`VHf%;U%o&3$z(>HxTxG5>A2S~^?=$Z* z7nvF6%x6Eq<3x2L2Et|wLly=tqO7IH1 z#r-WQclA~L&3pjPF-3G0{KTFut1)tS;5h)g%~^ECQu#pEYDRHhduP{ zDfBX^HSV24u6C3E{1Raf)PA(`EApD*>Iw>a!QM}O_!+jk%#<;o(@BkSJh!`Ts{V8v z%?5BIO|p2af4EQ>=Y~@w@$^XUjZi*5HW`X?heCb5{SxiLcn4iB$J^-=#2#7<;7Wt5 zz`uQs{LX1g7VuV|`-Xea?I-i3!}X(U$oa@QZ7xBFvt_c>%s66Iy9N5isd23`|2Mb-bUIFQMu5|0&ACC)71*_B+c80JKM%}SEQ>#}J2 zf?hv}>*!)X_S2)=P&w_rh}f1?De&K^g20QaEJm~qQxm18C;!tjup&f7i4SXz?vk68#(HNII{|g8AA#3CCf%q29vZyUGwdGD9Npjd`*vq$XJ&VrCfZEfu@ItAcUrYF#HGj-+7-G5k}d9H$^K~vtfRAZ z@*o%DA$YiLA<5mJ;3o}x?BJG{qxhxg!lp5=Mg;!)Um(0&Ov zmT!LViCKpbyIF1~TbeFUW^={crO85hnzhw#^yX8hleC@lgNfvzVfH2qCU1`ZPM#btT8wo)l1-Dt+4t2{BxTUqp?<6&JP5-Mrh6TmL7w}P=4_oSC z;cl}90M{Qb0X2RyF5DgVZ~}6|NMT&|8(r~e6kD7{FVj*QKS)I`2QE~BDj7UND`}rl z=LhNdIGzye8T@R6L(65HfUl(8!C38pHx;G%IlK-*u``FEl&?QtBex;=WbX?azHRMc zrvVpNOnyo;mB4>g3+I%;m7$?~9o1o~#CZo5G~T7o&KB*dLcWyEyEM#sKO4)?CzU(2 v#dwswIFATh=ilC-sSJ?R;9dS(VMPpV^7kQ`>GeQlORxLcQ(e@5@vG)PFItX} diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py index b5ae58e60..7c690772c 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py @@ -1,17 +1,18 @@ import json import cv2 +import math import base64 -import numpy as np -import matplotlib.pyplot as plt import threading import time +from datetime import datetime import websocket -from src.manager.ram_logging.log_manager import LogManager -from shared.image import SharedImage import rclpy +import numpy as np +import matplotlib.pyplot as plt +from hal_interfaces.general.odometry import OdometryNode +from shared.image import SharedImage from console import start_console from map import Map -from HAL import getPose3d # Graphical User Interface Class @@ -24,85 +25,103 @@ indigo = [130, 0, 75] violet = [211, 0, 148] -class ThreadingGUI: - def __init__(self, host="ws://127.0.0.1:2303", freq=30.0): +class GUI: + # Initialization function + # The actual initialization + def __init__(self, host): - # ROS 2 init - if not rclpy.ok(): - rclpy.init() + # ROS2 init + rclpy.init(args=None) + node = rclpy.create_node('GUI') - # Execution control vars - self.out_period = 1.0 / freq - self.ack = True - self.ack_lock = threading.Lock() - self.running = True - self.host = host - self.node = rclpy.create_node("node") - - # Payload vars - self.msg = {"pos_msg": "", "ang_msg": ""} + self.payload = {'map': '', 'user': ''} + self.server = None + self.client = None self.init_coords = (171, 63) self.start_coords = (201, 85.5) - self.map = Map(getPose3d) - # Image vars - self.shared_image = SharedImage("guiimage") - - # 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_loc", 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, incoming msgs can only be acks - if "ack" in message: - with self.ack_lock: - self.ack = True - - # 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 and self.map is not None: - self.send_map() - self.ack = False + self.host = host - # Maintain desired frequency - elapsed = time.time() - start_time - sleep_time = max(0, self.out_period - elapsed) - time.sleep(sleep_time) + self.ack = False + self.ack_lock = threading.Lock() - # Prepares and sends a map to the websocket server - def send_map(self): + self.shared_image = SharedImage("guiimage") - # Get the necessary info + self.pose3d_object = OdometryNode("/odom") + + # Spin nodes so that subscription callbacks load topic data + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(self.pose3d_object) + executor_thread = threading.Thread(target=executor.spin, daemon=True) + executor_thread.start() + + # Create the lap object + self.map = Map(self.pose3d_object) + + self.client_thread = threading.Thread(target=self.run_websocket) + self.client_thread.start() + + # Function to get value of Acknowledge + def get_acknowledge(self): + with self.ack_lock: + ack = self.ack + + return ack + + # Function to get value of Acknowledge + def set_acknowledge(self, value): + with self.ack_lock: + self.ack = value + + # encode the image data to be sent to websocket + def payloadImage(self): + + 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 + + # Update the gui + def update_gui(self): + # Payload Map Message pos_message = self.map.getRobotCoordinates() - if pos_message == self.init_coords: + if (pos_message == self.init_coords): pos_message = self.start_coords ang_message = self.map.getRobotAngle() - - self.msg["pos_msg"] = pos_message - self.msg["ang_msg"] = ang_message - message = json.dumps(self.msg) - try: - if self.client: + pos_message = str(pos_message + ang_message) + self.payload["map"] = pos_message + + # Example Payload Navigation Data message (random data) + # 4 colors supported (0, 1, 2, 3) + #nav_mat = np.zeros((20, 20), int) + #nav_mat[2, 1] = 1 + #nav_mat[3, 3] = 2 + #nav_mat[5,9] = 3 + #nav_message = str(nav_mat.tolist()) + 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: - LogManager.logger.info(f"Error sending message: {e}") + except Exception as e: + print(f"Error sending message: {e}") + + # Function to read the message from websocket + # Gets called when there is an incoming message from the client + def on_message(self, message): + # Acknowledge Message for GUI Thread + if (message[:4] == "#ack"): + self.set_acknowledge(True) def process_colors(self, image): colored_image = np.zeros((image.shape[0], image.shape[1], 3), dtype=np.uint8) @@ -135,16 +154,57 @@ def showNumpy(self, image): def getMap(self, url): return plt.imread(url) + def run_websocket(self): + while True: + self.client = websocket.WebSocketApp(self.host, on_message=self.on_message) + self.client.run_forever(ping_timeout=None, ping_interval=0) + + # Function to reset + def reset_gui(self): + self.map.reset() + + +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.iteration_counter = 0 + self.running = True + def start(self): + """Starts the GUI, frequency measurement, and real-time factor threads.""" + self.gui_thread = threading.Thread(target=self.run) + self.gui_thread.start() + print("GUI Thread Started!") + + 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) + + +# Create a GUI interface host = "ws://127.0.0.1:2303" -gui = ThreadingGUI(host) +gui_interface = GUI(host) + +# Spin a thread to keep the interface updated +thread_gui = ThreadGUI(gui_interface) +thread_gui.start() + +start_console() # Expose to the user def showNumpy(image): - gui.showNumpy(image) + gui_interface.showNumpy(image) def getMap(self, url): - return gui.image(url) - -# Redirect the console -start_console() + return gui_interface.image(url) \ No newline at end of file diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py index e89fc99e9..dec52ee53 100755 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/map.py @@ -3,79 +3,53 @@ from math import pi as pi import cv2 - class Map: - def __init__(self, pose_getter): - self.pose_getter = pose_getter - - def RTx(self, angle, tx, ty, tz): - RT = np.matrix( - [ - [1, 0, 0, tx], - [0, math.cos(angle), -math.sin(angle), ty], - [0, math.sin(angle), math.cos(angle), tz], - [0, 0, 0, 1], - ] - ) - return RT - - def RTy(self, angle, tx, ty, tz): - RT = np.matrix( - [ - [math.cos(angle), 0, math.sin(angle), tx], - [0, 1, 0, ty], - [-math.sin(angle), 0, math.cos(angle), tz], - [0, 0, 0, 1], - ] - ) - return RT - - def RTz(self, angle, tx, ty, tz): - RT = np.matrix( - [ - [math.cos(angle), -math.sin(angle), 0, tx], - [math.sin(angle), math.cos(angle), 0, ty], - [0, 0, 1, tz], - [0, 0, 0, 1], - ] - ) - return RT - - def RTVacuum(self): - RTz = self.RTz(pi / 2, 50, 70, 0) - return RTz - - def getRobotCoordinates(self): - pose = self.pose_getter() - while pose != None: - pose = self.pose_getter() - print(pose) - x = pose.x - y = pose.y - - scale_y = 15 - offset_y = 63 - y = scale_y * y + offset_y - - scale_x = -30 - offset_x = 171 - x = scale_x * x + offset_x - - return x, y - - def getRobotAngle(self): - pose = self.pose_getter() - while pose != None: - pose = self.pose_getter() - print(pose) - rt = pose.yaw - - ty = math.cos(-rt) - math.sin(-rt) - tx = math.sin(-rt) + math.cos(-rt) - - return tx, ty - - # Function to reset - def reset(self): - # Nothing to do, service takes care! - pass + def __init__(self, pose3d): + self.pose3d = pose3d + + def RTx(self, angle, tx, ty, tz): + RT = np.matrix([[1, 0, 0, tx], [0, math.cos(angle), -math.sin(angle), ty], + [0, math.sin(angle), math.cos(angle), tz], [0, 0, 0, 1]]) + return RT + + def RTy(self, angle, tx, ty, tz): + RT = np.matrix([[math.cos(angle), 0, math.sin(angle), tx], [0, 1, 0, ty], + [-math.sin(angle), 0, math.cos(angle), tz], [0, 0, 0, 1]]) + return RT + + def RTz(self, angle, tx, ty, tz): + RT = np.matrix([[math.cos(angle), -math.sin(angle), 0, tx], [math.sin(angle), math.cos(angle), 0, ty], + [0, 0, 1, tz], [0, 0, 0, 1]]) + return RT + + def RTVacuum(self): + RTz = self.RTz(pi/2, 50, 70, 0) + return RTz + + def getRobotCoordinates(self): + pose = self.pose3d.getPose3d() + x = pose.x + y = pose.y + + scale_y = 15; offset_y = 63 + y = scale_y * y + offset_y + + scale_x = -30; offset_x = 171 + x = scale_x * x + offset_x + + return x, y + + def getRobotAngle(self): + pose = self.pose3d.getPose3d() + rt = pose.yaw + + ty = math.cos(-rt) - math.sin(-rt) + tx = math.sin(-rt) + math.cos(-rt) + + return tx, ty + + # Function to reset + def reset(self): + # Nothing to do, service takes care! + pass + diff --git a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js index 3104c37a5..262767b66 100644 --- a/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js @@ -9,20 +9,26 @@ export default function SpecificVacuumCleaner() { console.log("TestShowScreen subscribing to ['update'] events"); const callback = (message) => { - console.log(message); - // if (data.map) { - // const pose = data.map.substring(1, data.map.length - 1); - // const content = pose.split(",").map(function (item) { - // return parseFloat(item); - // }); - // draw( - // guiCanvasRef.current, - // content[0], - // content[1], - // content[2], - // content[3] - // ); - // } + const updateData = message.data.update; + // Lógica para manejar el mapa + if (updateData.map) { + const pose = updateData.map.substring(1, updateData.map.length - 1); + const content = pose.split(",").map(item => parseFloat(item)); + const poseUser = updateData.user.substring(1, updateData.user.length - 1); + const userContent = poseUser.split(",").map(item => parseFloat(item)); + + draw( + guiCanvasRef.current, + content[0], + content[1], + content[2], + content[3], + userContent[0], + userContent[1], + userContent[2], + userContent[3] + ); + } }; window.RoboticsExerciseComponents.commsManager.subscribe( @@ -39,6 +45,29 @@ export default function SpecificVacuumCleaner() { }; }, []); + React.useEffect(() => { + const callback = (message) => { + if (message.data.state === "ready") { + try { + clearMap(guiCanvasRef.current,) + } catch (error) { + } + } + } + window.RoboticsExerciseComponents.commsManager.subscribe( + [window.RoboticsExerciseComponents.commsManager.events.STATE_CHANGED], + callback + ); + + return () => { + console.log("TestShowScreen unsubscribing from ['state-changed'] events"); + window.RoboticsExerciseComponents.commsManager.unsubscribe( + [window.RoboticsExerciseComponents.commsManager.events.STATE_CHANGED], + callback + ); + }; + }, []) + return ( Date: Sun, 26 May 2024 20:13:01 +0530 Subject: [PATCH 08/12] Migration of 3d_reconstruction to ros2 superthin templates (#2515) *migration of 3d_reconstruction to ros2 --- .../python_template/ros1_noetic/exercise.py | 152 ---------------- .../python_template/ros1_noetic/hal.py | 97 ----------- .../interfaces/camera_parameters.py | 164 ------------------ .../ros1_noetic/interfaces/threadPublisher.py | 46 ----- .../3d_reconstruction_conf.yml | 0 .../gui.py => ros2_humble/GUI.py} | 81 ++++++--- .../python_template/ros2_humble/HAL.py | 92 ++++++++++ .../{ros1_noetic => ros2_humble}/README.md | 0 .../code/academy.py | 0 .../{ros1_noetic => ros2_humble}/console.py | 0 .../interfaces/__init__.py | 0 .../interfaces/camera.py | 14 +- 12 files changed, 153 insertions(+), 493 deletions(-) delete mode 100644 exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/exercise.py delete mode 100644 exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/hal.py delete mode 100644 exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/camera_parameters.py delete mode 100644 exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/threadPublisher.py rename exercises/static/exercises/3d_reconstruction_newmanager/python_template/{ros1_noetic => ros2_humble}/3d_reconstruction_conf.yml (100%) rename exercises/static/exercises/3d_reconstruction_newmanager/python_template/{ros1_noetic/gui.py => ros2_humble/GUI.py} (84%) create mode 100644 exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/HAL.py rename exercises/static/exercises/3d_reconstruction_newmanager/python_template/{ros1_noetic => ros2_humble}/README.md (100%) rename exercises/static/exercises/3d_reconstruction_newmanager/python_template/{ros1_noetic => ros2_humble}/code/academy.py (100%) rename exercises/static/exercises/3d_reconstruction_newmanager/python_template/{ros1_noetic => ros2_humble}/console.py (100%) rename exercises/static/exercises/3d_reconstruction_newmanager/python_template/{ros1_noetic => ros2_humble}/interfaces/__init__.py (100%) rename exercises/static/exercises/3d_reconstruction_newmanager/python_template/{ros1_noetic => ros2_humble}/interfaces/camera.py (95%) diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/exercise.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/exercise.py deleted file mode 100644 index 222f53c49..000000000 --- a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/exercise.py +++ /dev/null @@ -1,152 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function -import time -import threading -import sys -from datetime import datetime -import re -import importlib -from gui import GUI, ThreadGUI -from hal import HAL -from console import start_console, close_console - -class Template: - """ - Main class for executing and controlling user code. - """ - - def __init__(self): - """ - Initializes the Template class, setting up the environment for user code execution. - """ - self.measure_thread = None - self.thread = None - self.reload = False - self.stop_brain = False - self.user_code = "" - self.ideal_cycle = 20 - - self.hal = HAL() - self.gui = GUI("0.0.0.0", self.hal) - self.execute_user_code() - - def parse_code(self, source_code): - """ - Parses the provided source code, separating the sequential and iterative parts. - """ - sequential_code, iterative_code = self.seperate_seq_iter(source_code) - return iterative_code, sequential_code - - def seperate_seq_iter(self, source_code): - """ - Separates the source code into sequential and iterative components. - """ - if source_code == "": - return "", "" - - infinite_loop = re.search( - r'[^ ]while\s*\(\s*True\s*\)\s*:|[^ ]while\s*True\s*:|[^ ]while\s*1\s*:|[^ ]while\s*\(\s*1\s*\)\s*:', source_code) - - try: - start_index = infinite_loop.start() - iterative_code = source_code[start_index:] - sequential_code = source_code[:start_index] - - iterative_code = re.sub( - r'[^ ]while\s*\(\s*True\s*\)\s*:|[^ ]while\s*True\s*:|[^ ]while\s*1\s*:|[^ ]while\s*\(\s*1\s*\)\s*:', '', iterative_code) - extra_lines = sequential_code.count('\n') - while (extra_lines >= 0): - iterative_code = '\n' + iterative_code - extra_lines -= 1 - iterative_code = re.sub(r'^[ ]{4}', '', iterative_code, flags=re.M) - - except: - sequential_code = source_code - iterative_code = "" - - return sequential_code, iterative_code - - def process_code(self, source_code): - """ - Processes the provided source code, executing the sequential part first and then the iterative part. - """ - start_console() - - iterative_code, sequential_code = self.parse_code(source_code) - - gui_module, hal_module = self.generate_modules() - reference_environment = {"GUI": gui_module, "HAL": hal_module} - exec(sequential_code, reference_environment) - - while self.reload == False: - while (self.stop_brain == True): - if (self.reload == True): - break - time.sleep(0.1) - - start_time = datetime.now() - - exec(iterative_code, reference_environment) - - finish_time = datetime.now() - dt = finish_time - start_time - ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 - - if (ms < self.ideal_cycle): - time.sleep((self.ideal_cycle - ms) / 1000.0) - - close_console() - print("Current Thread Joined!") - - # Function to generate the modules for use in ACE Editor - def generate_modules(self): - # Define HAL module - hal_module = importlib.util.module_from_spec(importlib.machinery.ModuleSpec("HAL", None)) - hal_module.HAL = importlib.util.module_from_spec(importlib.machinery.ModuleSpec("HAL", None)) - - # Add HAL functions - hal_module.HAL.getImage = self.hal.getImage - hal_module.HAL.graficToOptical = self.hal.graficToOptical - hal_module.HAL.backproject = self.hal.backproject - hal_module.HAL.getCameraPosition = self.hal.getCameraPosition - hal_module.HAL.project = self.hal.project - hal_module.HAL.opticalToGrafic = self.hal.opticalToGrafic - hal_module.HAL.project3DScene = self.hal.project3DScene - - # Define GUI module - gui_module = importlib.util.module_from_spec(importlib.machinery.ModuleSpec("GUI", None)) - gui_module.GUI = importlib.util.module_from_spec(importlib.machinery.ModuleSpec("GUI", None)) - - # Add GUI functions - gui_module.GUI.showImages = self.gui.showImages - gui_module.GUI.ShowNewPoints = self.gui.ShowNewPoints - gui_module.GUI.ShowAllPoints = self.gui.ShowAllPoints - gui_module.GUI.showImageMatching = self.gui.showImageMatching - gui_module.GUI.ClearAllPoints = self.gui.ClearAllPoints - - # Adding modules to system - # Protip: The names should be different from - # other modules, otherwise some errors - sys.modules["HAL"] = hal_module - sys.modules["GUI"] = gui_module - - return gui_module, hal_module - - def execute_user_code(self): - """ - Executes the provided user code, initializing the graphical interface and processing the code. - """ - with open('/workspace/code/academy.py', 'r') as code_file: - source_code = code_file.read() - - self.thread_gui = ThreadGUI(self.gui) - self.thread_gui.start() - - self.thread = threading.Thread( - target=self.process_code, args=[source_code]) - self.thread.start() - print("Código del usuario en ejecución.") - -if __name__ == "__main__": - server = Template() \ No newline at end of file diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/hal.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/hal.py deleted file mode 100644 index af1eea2b8..000000000 --- a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/hal.py +++ /dev/null @@ -1,97 +0,0 @@ -import rospy -import cv2 -import threading -import time -from datetime import datetime -import math -import numpy as np -from interfaces.camera import ListenerCamera, ListenerParameters - - -# Hardware Abstraction Layer -class HAL: - IMG_WIDTH = 640 - IMG_HEIGHT = 480 - - def __init__(self): - rospy.init_node("HAL") - - self.image = None - self.cameraL = ListenerCamera("/TurtlebotROS/cameraL/image_raw") - self.cameraR = ListenerCamera("/TurtlebotROS/cameraR/image_raw") - - self.camLeftP = ListenerParameters("3d_reconstruction_conf.yml", "CamACalibration") - self.camRightP = ListenerParameters("3d_reconstruction_conf.yml", "CamBCalibration") - - # Get Image from ROS Driver Camera - def getImage(self, lr): - if (lr == 'left'): - image = self.cameraL.getImage().data - elif (lr == 'right'): - image = self.cameraR.getImage().data - else: - print("Invalid camera") - - return image - - # Transform the Coordinate System to the Camera System - def graficToOptical(self, lr, point2d): - if (lr == 'left'): - pointOpt = self.camLeftP.graficToOptical(point2d) - elif (lr == 'right'): - pointOpt = self.camRightP.graficToOptical(point2d) - else: - print("Invalid camera") - return pointOpt - - # Backprojects the 2D Point into 3D Space - def backproject(self, lr, point2d): - if (lr == 'left'): - point3D = self.camLeftP.backproject(point2d) - elif (lr == 'right'): - point3D = self.camRightP.backproject(point2d) - else: - print("Invalid camera") - return point3D - - # Get Camera Position from ROS Driver Camera - def getCameraPosition(self, lr): - if (lr == 'left'): - position_cam = self.camLeftP.getCameraPosition() - elif (lr == 'right'): - position_cam = self.camRightP.getCameraPosition() - else: - print("Invalid camera") - - return position_cam - - # Backprojects a 3D Point onto an Image - def project(self, lr, point3d): - if (lr == 'left'): - projected = self.camLeftP.project(point3d) - elif (lr == 'right'): - projected = self.camRightP.project(point3d) - else: - print("Invalid camera") - - return projected - - # Get Image Coordinates - def opticalToGrafic(self, lr, point2d): - if (lr == 'left'): - point = self.camLeftP.opticalToGrafic(point2d) - elif (lr == 'right'): - point = self.camRightP.opticalToGrafic(point2d) - else: - print("Invalid camera") - - return point - - - def project3DScene(self,point3d): - px = point3d[0] / 100.0 - py = point3d[1] / 100.0 + 12 - pz = point3d[2] / 100.0 - outPoint = np.array([px,py,pz]); - return outPoint - diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/camera_parameters.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/camera_parameters.py deleted file mode 100644 index 00c50934e..000000000 --- a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/camera_parameters.py +++ /dev/null @@ -1,164 +0,0 @@ -import rospy -from sensor_msgs.msg import CameraInfo -import numpy as np -import config -from numpy.linalg import inv - -def param2Msg(K, RT, width, height): - param = CameraInfo() - param.height = height - param.width = width - param.K = K - param.R = RT - - return param - -import numpy as np - -class CameraParameters: - - def __init__(self): - self.height = 3 - self.width = 3 - self.timeStamp = 0 - self.K = np.zeros(9) - self.RT = np.zeros(9) - - def __str__(self): - s = "Camera: {\n height: " + str(self.height) + "\n width: " + str(self.width) - s = s + "\n K: " + str(self.K) + "\n RT: " + str(self.RT) - s = s + "\n timeStamp: " + str(self.timeStamp) - - return s - -class PublisherCamera: - - def __init__(self, configFile, cam): - cfg = config.load(configFile) - data = cfg.getProperty("3DReconstruction."+cam+".data") - - self.K=np.array([data["K"][0],data["K"][1],data["K"][2],data["K"][4], data["K"][5],data["K"][6],data["K"][8],data["K"][9],data["K"][10]],dtype=np.double).reshape(3,3) - self.RT=np.array([data["RT"][0],data["RT"][1],data["RT"][2],data["RT"][3], data["RT"][4],data["RT"][5],data["RT"][6],data["RT"][7],data["RT"][8],data["RT"][9],data["RT"][10],data["RT"][11],0,0,0,1],dtype=np.double).reshape(4,4) - self.width=data["Size"][0] - self.height=data["Size"][1] - - def backproject(self, point2d): - myin_h=self.K[0,0] - myin_x=point2d[0]*self.K[0,0]/point2d[2] - myin_y=point2d[1]*self.K[0,0]/point2d[2] - - ik11=(1./self.K[0,0]) - ik12=-self.K[0,1]/(self.K[0,0]*self.K[1,1]) - ik13=(self.K[0,1]*self.K[1,2]-self.K[0,2]*self.K[1,1])/(self.K[1,1]*self.K[0,0]) - ik21=0. - ik22=(1./self.K[1,1]) - ik23=-(self.K[1,2]/self.K[1,1]) - ik31=0. - ik32=0. - ik33=1. - - a1=ik11*myin_x+ik12*myin_y+ik13*myin_h - a2=ik21*myin_x+ik22*myin_y+ik23*myin_h - a3=ik31*myin_x+ik32*myin_y+ik33*myin_h - a4=1. - - ir11=self.RT[0,0] - ir12=self.RT[1,0] - ir13=self.RT[2,0] - ir14=0. - ir21=self.RT[0,1] - ir22=self.RT[1,1] - ir23=self.RT[2,1] - ir24=0. - ir31=self.RT[0,2] - ir32=self.RT[1,2] - ir33=self.RT[2,2] - ir34=0. - ir41=0. - ir42=0. - ir43=0. - ir44=1. - - b1=ir11*a1+ir12*a2+ir13*a3+ir14*a4 - b2=ir21*a1+ir22*a2+ir23*a3+ir24*a4 - b3=ir31*a1+ir32*a2+ir33*a3+ir34*a4 - b4=ir41*a1+ir42*a2+ir43*a3+ir44*a4 - - it11=1. - it12=0. - it13=0. - it14=-self.RT[2,3] - it21=0.; it22=1.; it23=0.; it24=self.RT[1,3]; - it31=0.; it32=0.; it33=1.; it34=-self.RT[0,3]; - it41=0.; it42=0.; it43=0.; it44=1.; - - outPoint = np.array([0,0,0,1]) - outPoint[0]=it11*b1+it12*b2+it13*b3+it14*b4; - outPoint[1]=it21*b1+it22*b2+it23*b3+it24*b4; - outPoint[2]=it31*b1+it32*b2+it33*b3+it34*b4; - outPoint[3]=it41*b1+it42*b2+it43*b3+it44*b4; - return outPoint - - def backproject2(self, point2d): - iK = inv(self.K) - Pi=np.array([point2d[0]/point2d[2],point2d[1]/point2d[2],1.0],dtype=np.double).reshape(3,1) - a=np.dot(iK,Pi) - aH=np.array([a[0],a[1],a[2],1],dtype=np.double) - RT2=self.RT.copy() - RT2[0,3] = 0 - RT2[1,3] = 0 - RT2[2,3] = 0 - RT2[3,3] = 1 - b = np.dot(np.transpose(RT2),aH) - translate = np.identity(4,dtype=np.double) - translate[0,3] = self.RT[0,3]/self.RT[3,3]; - translate[1,3] = self.RT[1,3]/self.RT[3,3]; - translate[2,3] = self.RT[2,3]/self.RT[3,3]; - b=np.dot(translate,b) - outPoint = np.array([b[0]/b[3],b[1]/b[3],b[2]/b[3],1]) - return outPoint - - def project(self,point3d): - - a1=self.RT[0,0]*point3d[0]+self.RT[0,1]*point3d[1]+self.RT[0,2]*point3d[2]+self.RT[0,3]*point3d[3] - a2=self.RT[1,0]*point3d[0]+self.RT[1,1]*point3d[1]+self.RT[1,2]*point3d[2]+self.RT[1,3]*point3d[3] - a3=self.RT[2,0]*point3d[0]+self.RT[2,1]*point3d[1]+self.RT[2,2]*point3d[2]+self.RT[2,3]*point3d[3] - a4=self.RT[3,0]*point3d[0]+self.RT[3,1]*point3d[1]+self.RT[3,2]*point3d[2]+self.RT[3,3]*point3d[3] - out_x=self.K[0,0]*a1+self.K[0,1]*a2+self.K[0,2]*a3; - out_y=self.K[1,0]*a1+self.K[1,1]*a2+self.K[1,2]*a3; - out_h=self.K[2,0]*a1+self.K[2,1]*a2+self.K[2,2]*a3; - - if out_h!=0.: - out_x=out_x/out_h; - out_y=out_y/out_h; - out_h=1.; - return np.array([out_x,out_y,out_h]) - - - def project2(self,point3d): - a1=self.RT[0,0]*point3d[0] + self.RT[0,1]*point3d[1] + self.RT[0,2]*point3d[2] - self.RT[2,3] - a2=self.RT[1,0]*point3d[0] + self.RT[1,1]*point3d[1] + self.RT[1,2]*point3d[2] + self.RT[1,3] - a3=self.RT[2,0]*point3d[0] + self.RT[2,1]*point3d[1] + self.RT[2,2]*point3d[2] - self.RT[0,3] - aP= np.array([a1,a2,a3],dtype=np.double) - # a = self.RT.dot(point3d) - # print ("aproject") - # print (a) - # aP = np.array([a[0]/a[3],a[1]/a[3],a[2]/a[3]],dtype=np.double) - p = self.K.dot(aP) - outPoint = np.array([p[0]/p[2],p[1]/p[2],1.0]); - return outPoint - - def graficToOptical(self,point2d): - x = point2d[0] - y = point2d[1] - point = np.array([self.height-1-y,x, point2d[2]]) - return point - - def opticalToGrafic(self,point2d): - x = point2d[0] - y = point2d[1] - point = np.array([y,self.height - 1 - x, point2d[2]]) - return point - - def getCameraPosition(self): - return np.array([-self.RT[2,3],self.RT[1,3],-self.RT[0,3]]) diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/threadPublisher.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/threadPublisher.py deleted file mode 100644 index 69aa0ad48..000000000 --- a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/threadPublisher.py +++ /dev/null @@ -1,46 +0,0 @@ -# -# Copyright (C) 1997-2016 JDE Developers Team -# -# This program is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program. If not, see http://www.gnu.org/licenses/. -# Authors : -# Alberto Martin Florido -# Aitor Martinez Fernandez -# -import threading -import time -from datetime import datetime - -time_cycle = 80 - - -class ThreadPublisher(threading.Thread): - - def __init__(self, pub, kill_event): - self.pub = pub - self.kill_event = kill_event - threading.Thread.__init__(self, args=kill_event) - - def run(self): - while (not self.kill_event.is_set()): - start_time = datetime.now() - - self.pub.publish() - - finish_Time = datetime.now() - - dt = finish_Time - start_time - ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 - #print (ms) - if (ms < time_cycle): - time.sleep((time_cycle - ms) / 1000.0) \ No newline at end of file diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/3d_reconstruction_conf.yml b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/3d_reconstruction_conf.yml similarity index 100% rename from exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/3d_reconstruction_conf.yml rename to exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/3d_reconstruction_conf.yml diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/gui.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/GUI.py similarity index 84% rename from exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/gui.py rename to exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/GUI.py index d1d21b770..13cc83bae 100644 --- a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/gui.py +++ b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/GUI.py @@ -1,3 +1,4 @@ +import rclpy import json import cv2 import base64 @@ -9,13 +10,20 @@ import os import numpy as np +from console import start_console + # Graphical User Interface Class class GUI: # Initialization function # The actual initialization - def __init__(self, host, hal): + def __init__(self, host): self.payload = {'img1': '', 'img2': '', 'pts': '', 'match': '', 'p_match': 'F'} + + # ROS2 init + rclpy.init(args=None) + node = rclpy.create_node('GUI') + self.server = None self.client = None @@ -27,8 +35,8 @@ def __init__(self, host, hal): self.image_to_be_shown_updated = False self.image_show_lock = threading.Lock() - self.acknowledge = False - self.acknowledge_lock = threading.Lock() + self.ack = False + self.ack_lock = threading.Lock() self.point_to_save = [] self.point_to_send = [] @@ -38,27 +46,22 @@ def __init__(self, host, hal): self.matching_to_send = [] self.paint_matching = "F" - - # Get HAL object - self.hal = hal 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 = websocket.WebSocketApp(self.host, on_message=self.on_message,) self.client.run_forever(ping_timeout=None, ping_interval=0) # 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 - image1_to_be_shown = self.image1_to_be_shown - image2_to_be_shown = self.image2_to_be_shown - self.image_show_lock.release() + with self.image_show_lock: + image_to_be_shown_updated = self.image_to_be_shown_updated + image1_to_be_shown = self.image1_to_be_shown + image2_to_be_shown = self.image2_to_be_shown image1 = image1_to_be_shown payload1 = {'img': ''} @@ -78,9 +81,9 @@ def payloadImage(self): encoded_image2 = base64.b64encode(frame2) payload2['img'] = encoded_image2.decode('utf-8') - self.image_show_lock.acquire() - self.image_to_be_shown_updated = False - self.image_show_lock.release() + with self.image_show_lock: + self.image_to_be_shown_updated = False + return payload1, payload2 # Function for student to call @@ -91,25 +94,22 @@ def showImages(self, image1, image2, paint_matching): else: self.paint_matching = "F" if (np.all(self.image1_to_be_shown == image1) == False or np.all(self.image2_to_be_shown == image2) == False): - self.image_show_lock.acquire() - self.image1_to_be_shown = image1 - self.image2_to_be_shown = image2 - self.image_to_be_shown_updated = True - self.image_show_lock.release() + with self.image_show_lock: + self.image1_to_be_shown = image1 + self.image2_to_be_shown = image2 + self.image_to_be_shown_updated = True # Function to get value of Acknowledge def get_acknowledge(self): - self.acknowledge_lock.acquire() - acknowledge = self.acknowledge - self.acknowledge_lock.release() + with self.ack: + ack = self.acknowledge - return acknowledge + return ack # Function to get value of Acknowledge def set_acknowledge(self, value): - self.acknowledge_lock.acquire() - self.acknowledge = value - self.acknowledge_lock.release() + with self.ack_lock: + self.ack = value # Update the gui def update_gui(self): @@ -268,3 +268,28 @@ def run(self): sleep_time = max(0, (50 - ms) / 1000.0) time.sleep(sleep_time) + +# Create a GUI interface +host = "ws://127.0.0.1:2303" +gui_interface = GUI(host) + +# Spin a thread to keep the interface updated +thread_gui = ThreadGUI(gui_interface) +thread_gui.start() + +start_console() + +def showImages(image1, image2, paint_matching): + gui_interface.showImages(image1, image2, paint_matching) + +def ShowNewPoints(points): + gui_interface.ShowNewPoints(points) + +def ShowAllPoints(points): + gui_interface.ShowAllPoints(points) + +def showImageMatching(x1, y1, x2, y2): + gui_interface.showImageMatching(x1, y1, x2, y2) + +def ClearAllPoints(): + gui_interface.ClearAllPoints() diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/HAL.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/HAL.py new file mode 100644 index 000000000..dabb7a540 --- /dev/null +++ b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/HAL.py @@ -0,0 +1,92 @@ +import rclpy +from interfaces.camera import ListenerCamera, ListenerParameters +import numpy as np + + +# Hardware Abstraction Layer + +IMG_WIDTH = 640 +IMG_HEIGHT = 480 + +# ROS2 init +rclpy.create_node('HAL') + +image = None +cameraL = ListenerCamera("/cam_turtlebot_left/image_raw") +cameraR = ListenerCamera("/cam_turtlebot_right/image_raw") + +camLeftP = ListenerParameters("3d_reconstruction_conf.yml", "CamACalibration") +camRightP = ListenerParameters("3d_reconstruction_conf.yml", "CamBCalibration") + +# Get Image from ROS Driver Camera +def getImage(lr): + try: + if (lr == 'left'): + rclpy.spin_once(cameraL) + image = cameraL.getImage().data + elif (lr == 'right'): + rclpy.spin_once(cameraR) + image = cameraR.getImage().data + return image + except Exception as e: + print(f"Exception in hal getImage {repr(e)}") + +# Transform the Coordinate System to the Camera System +def graficToOptical(lr, point2d): + if (lr == 'left'): + pointOpt = camLeftP.graficToOptical(point2d) + elif (lr == 'right'): + pointOpt = camRightP.graficToOptical(point2d) + else: + print("Invalid camera") + return pointOpt + +# Backprojects the 2D Point into 3D Space +def backproject(lr, point2d): + if (lr == 'left'): + point3D = camLeftP.backproject(point2d) + elif (lr == 'right'): + point3D = camRightP.backproject(point2d) + else: + print("Invalid camera") + return point3D + +# Get Camera Position from ROS Driver Camera +def getCameraPosition(lr): + if (lr == 'left'): + position_cam = camLeftP.getCameraPosition() + elif (lr == 'right'): + position_cam = camRightP.getCameraPosition() + else: + print("Invalid camera") + + return position_cam + +# Backprojects a 3D Point onto an Image +def project(lr, point3d): + if (lr == 'left'): + projected = camLeftP.project(point3d) + elif (lr == 'right'): + projected = camRightP.project(point3d) + else: + print("Invalid camera") + + return projected + +# Get Image Coordinates +def opticalToGrafic(lr, point2d): + if (lr == 'left'): + point = camLeftP.opticalToGrafic(point2d) + elif (lr == 'right'): + point = camRightP.opticalToGrafic(point2d) + else: + print("Invalid camera") + + return point + +def project3DScene(point3d): + px = point3d[0] / 100.0 + py = point3d[1] / 100.0 + 12 + pz = point3d[2] / 100.0 + outPoint = np.array([px,py,pz]); + return outPoint diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/README.md b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/README.md similarity index 100% rename from exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/README.md rename to exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/README.md diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/code/academy.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/code/academy.py similarity index 100% rename from exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/code/academy.py rename to exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/code/academy.py diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/console.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/console.py similarity index 100% rename from exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/console.py rename to exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/console.py diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/__init__.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/interfaces/__init__.py similarity index 100% rename from exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/__init__.py rename to exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/interfaces/__init__.py diff --git a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/camera.py b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/interfaces/camera.py similarity index 95% rename from exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/camera.py rename to exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/interfaces/camera.py index 76c594200..60721042b 100644 --- a/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/interfaces/camera.py +++ b/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/interfaces/camera.py @@ -1,5 +1,6 @@ import numpy as np -import rospy +import rclpy +from rclpy.node import Node from sensor_msgs.msg import Image as ImageROS import yaml import threading @@ -19,7 +20,7 @@ def imageMsg2Image(img, bridge): image.width = img.width image.height = img.height image.format = "BGR8" - image.timeStamp = img.header.stamp.secs + (img.header.stamp.nsecs * 1e-9) + image.timeStamp = img.header.stamp.sec + (img.header.stamp.nanosec * 1e-9) cv_image = 0 if (img.encoding[-2:] == "C1"): gray_img_buff = bridge.imgmsg_to_cv2(img, img.encoding) @@ -56,7 +57,7 @@ def __init__(self, configFile, cam): if os.getcwd() == "/": f = open( - "/RoboticsAcademy/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros1_noetic/" + configFile, "r") + "/RoboticsAcademy/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/" + configFile, "r") else: f = open(configFile, "r") @@ -179,10 +180,11 @@ def getCameraPosition(self): return np.array([self.RT[0, 3], self.RT[1, 3], self.RT[2, 3]]) -class ListenerCamera: +class ListenerCamera(Node): def __init__(self, topic): - + + super().__init__("ListenerCamera") self.topic = topic self.data = Image() self.sub = None @@ -205,7 +207,7 @@ def stop(self): def start(self): - self.sub = rospy.Subscriber(self.topic, ImageROS, self.__callback) + self.create_subscription(ImageROS, self.topic, self.__callback, 10) def getImage(self): From 882db406d198a37cc12ccb3ea56fdd6ffcb15b12 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Sun, 26 May 2024 20:58:02 +0200 Subject: [PATCH 09/12] Working, but no Reset --- db.sqlite3 | Bin 327680 -> 327680 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/db.sqlite3 b/db.sqlite3 index dab8ac8274670bb87d1fe6a9875ba22654450a27..783c0cbfcb2ca7947e8bae61d6070867af03e2ad 100644 GIT binary patch delta 1534 zcmah}YiLtv7(U-6=bSW`lf;HLy`-@nD!8U6=j7zpWi_*#HEpJC)m50YY12#6G-)nv zEmN9ke@vWCC*%9GL4^szU~Fr*Gp57BaQ+eL9HMnMi(|+nw&0(^F6<qg%d-`gW}FEFnFLKa6o&=Etb$t*7Uoh09VFUE zKqq;h0(d-bj}*6}01$Y=*~q&Zd5=wS_!LT!x4Rr}kJmj?{7e=e;wpLPBXIE&YGkg+ z%qLI_(H-U*v3>@3sFs=UnJaS}`F_<#8Ue_{x}ssAb>{UiXcj^9Ap9d$^IrJsCr9EC&uBnjI zci~9@(n@LxKo%cBET`|mcV9;)wwW;7Py=L}6-gXRM@$Um&5#d5sFKXLBMZ46LPi31 zBCC9^4fR*BCOu1Wy?RzI>qUR*NN*BZXu-4@bp!HI8)}e)3G@v>%iIz%j3G1lZqAtN z=g9GCw9{zD6aan`O&6_KMXSiYX|&PmW_1+z1NQ}YhKqAMxO)BH`rGhol$MBXtzI8r=BE)}p%e)BDKz-=;_84OB-O4fr_^6!_?A9Xsj3MsGY z;?tudE5D$6^KI@1_bqpgTj0Lp=DCl#3*0LAvy$Rbj$BwlKE0`gQBqSd3eV~aG8!!G z*VG2kLFpIa3%Y{Np>5Sf%HC6cQOD<1SOD7ms9#OJ+!XD%_{SfD9to2g&&!!B&%IDRXz;X?>w(>ELn%d#_Pg?RVBvprNNnDGq z@(C@TXi#(Q)CPSYrSFr=c-*6}GNUH&Buq9v-k*&o6aGph0b+O?mXp&j;)`a5x48em zYxJi0Or4qvaLpq?{a>RvxqPB%_c+{= z+I2NIoc3c zg^ukFJgf9cxEgLCD+PRR`lVoZ*f%zs9&)v}OQYiM?c!8Wigt=Y*WkWB*TMaL;m&BL zD>gpdaVXW(9oqk7oUNO7M;k^;!JpMq_JQD`eX-WeuE|)SlO(DkT~ZPZOeMlTF`15a z1Ol(@YJIhHU}|)KOxT?jGi|MVLVZ(SA>PwH);YL0G9v8T9eAQ--C&jH3%JAdFXySC ArT_o{ delta 1496 zcmah}U2GIp6yEzYGdtVaKe{aRf4gg{M!TJvotd3kSxbT1mWqjOuq0Y((`~WrcDLJZ zDQXB?x({HamZ03kC!2_o_@Wja6HuEF!wWG*gXJf|0*NM)(nen}YU1r|npD9!H@RQ# zo%5aVoO|y%`MfWm_f1!T+b1fwgWI3(nB2!ufF-xM@3~7HU2tcJ|5Mn)T<5;wzM8ES z8<{wZ0OVk(2*D!Mu7Vu~icnMBtS&-TakJ7|nLnAZdNcCzM@?=XB53E|<=^I~=+f~W zJiw9nu0f_W#2_BRpf%T8bHJoO5-d;&vj}$uP#arscY#-ga{EQDgza$t!Jh`l`8@gp zEC|OOK67Ld9tU6-&Attg`MVI;_KmAKDU;YYAdO~o2UA1Zn4#xJ`cq@C4JGCA{R2re zeFvU;33=@^gxiWjG*`xsg`2jKlUYyNoM}aUn^>$8s`!ewiqmAI!tT*51CdUq=HZO;50K{3AT}6Xo{p8MJR9mjvyKL?o?$6yP z+$nd9JLvk$b=5WNnsN=A!92Rlk-48?2RXll1Lo^TQFn9$%W)W39xT+c-~8okbic;Q zF^F!ntF!JW%RizZxJf*V$VY=X;wB$1qnL}ej2O#^5?7Xy*Hb1KwothQR&oj8toZ?tPr*`$#kWl)b>7wzZyYb1Xe8MhTt$WiTy4G~;PDVoO-fw{ zZSHo{A>utQs_Vy_kqU_CP3R-jPvdhNeW)I+Y2-%6`f`JrblmJ|#P3g|3TBdU^SI7? z*}m6a!JXxr*&o;*=67b2UScy-H(H<)P2Ttz;DId4O1-Go%Su2}W0D?|qhXy^j;fMj z11aR*TTsxR32ONk-OxDAyqG{1+(Lv9P*hv*@AF8Ot zA_^_ourBGUX8cpJD>ITxtSP1n0qVg0RC2vyBp?~93⩽36KPz)a$TxW z^WOwf3`?3KMfG)rM4|e@oUDeTiZMwJM|54)m12m2Qsk^_DpXkn@A{C|I*_V`=@~@y z$SR@Yae8K%LZuZ@_F?7f>J6?Q78huIsZmQ Date: Sun, 26 May 2024 22:13:34 +0200 Subject: [PATCH 10/12] Trying to solve db issue --- db.sqlite3 | Bin 327680 -> 327680 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/db.sqlite3 b/db.sqlite3 index 783c0cbfcb2ca7947e8bae61d6070867af03e2ad..88f630f32426c8a6f37231cb29bbf1e0bd28809b 100644 GIT binary patch delta 380 zcmWlUJ1>J_7(m|}Z!6!Ws*yHs9|qB(7qn3#ZV@%8!D13Zn}}9ZNfCp*jm01hk$6`B zz@>N=lV8vfK^rC;LYSM-GoIz-DCJVhbw zz!wgLBE5aP4{3ZzQ@`S?Tm$({9}5|WkHO~;)zR}y-*32AmQbECok8OTix%1gJ2hBB z=5=%3eu1*cDirJP*s(lP4mX@Uvl_%_HkY8w=osNST{9<0Eg|t*Ng`PojdC|!P4iQk z5;iiJqmYSlPt|yU>!JZVW_Tyw$M}mygLt0ivkrII)(@v`7lKplvIW)A{k{gChn`nWQCR&{<+wo;yd|NE#7x1Pkltm9gq7$yBu O%l})YVg5s`H~ax{b9S)+ delta 827 zcmah{T}V@57(VCwKKHrybLO0>>3)MsbMx$+?d(UymW8IN=_)3nHFa}t(>dMzse~;J zx(G{%zN?^ax(dp_&P~t{c;|&Ak{S|%cV!CQ1R*r_Ch(&7;(d5ucpskkdEZn@lS*kG z7txzj#ntrY^sPM$t)Fc#51z{r3|5z$2x^lQ8Cm1{SKP(f{5P$1ps)FXl6hy!pnGJ*2s2dMie#R#kbLx^<7BQL7LsR;P)+DdP$w=o z!VLq>iPJIYRh@8TD=6kG^Ol)nqD&2AQtl~Nm9xspY=u~yf;*TjeFw-G<3s&J@uZFK z8x97B5~I=XL|Ab2$Jv2+q$4cEq9XSIejBDQ%&SqUTqDbf*pGDBDWz&ED~AiyRV1!Q zS`v{Xo$+>{%P{2cOeT8`ozZ~5FEDV&l<;>Cn%ssPrj{lzM;Z|-BU>~&OV$BmNJ@{C zqCtVSKEZqp2wp-Z611B3U<=(f;RH=5s+7v8BACA z#b(f(@F$Xkrna%MHVo2eLYtvtD$<2Vsd%@|Vc}Vq%SPV&PyxBWfehr&8Y&S-R?$N& zN3sy5TvyD=CuO0`PCnV0dncn=4dd55|FfR>i$E^f*+wtw&ewVU?*86^ZlS*3-pgO9 v=11%7J Date: Sun, 26 May 2024 22:17:18 +0200 Subject: [PATCH 11/12] Updated db --- db.sqlite3 | Bin 327680 -> 327680 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/db.sqlite3 b/db.sqlite3 index 88f630f32426c8a6f37231cb29bbf1e0bd28809b..8208a7551fe070ca2bd03d8d06cf998eb3d0238d 100644 GIT binary patch delta 682 zcmah{T}YE*6n_8a>)Q8iQvtki77&B%zxigk}*01)Ym?9v*l&=RD^ahQlx%4=UmI z!=q8SJ%4@Q2i=PU^{JK;&7-i!2Cb0=eQGEWYVgmjr^R|2)*DzyyPWR9_XzDSX6!QJ z(pM;#hW`+R)lg8?MS=hJ3fIvUL0=*nA-cjH3#c8@wZ&?to2*3`B8uDxr5L%yra-k7 z*k*nz=qlQW#QdxP$vgCzN+xVZ$o(_B#KH!;nu6oP-h%V8WE}!U03?ej2v6`xJFei% z-S~yw)(R*9AVnx>nmAnT(f0^J#q1j)TQArv|=^ zie)ii;OCUDenO-=mkA`N#-%7ay0%X{hiXLhASqB&YZ$A~tSR5v%aXWalj|r+H3S zNe!WUG+(f&n;zF^-DVIeXVs4ykqWvW;CeH+#9(}FCdFn%+P1Ds7F6Ti1XN=}ej z*C7zGHRYZEk|~;x1sDr!{B?{R;di!(hkxH7HDc-mxlbix83EQ!c}beHj1-25Sx(+Q zrklEF`Xm2yg2yXS8UM0No^+n=YQNDK9lOxkn$jly`nCSvOMQNCyd%S6lOw&MEAd<` d+LiU5*GAi-gW10RU%P2u_SYMUxz8k9_5(c|zoY;F delta 334 zcmWm7KTASU7=ZD6&U@-y<6bqDP*Rc_BnSUgN*b0Gkqz3=&{Q~-ONkhSPH`?R4Iw(c zTi+n+T5^`AK7kZcDK)iK5H$3%L9r1hD-N@4i?lL z7G;oeIB!wbQ>lv2SiBFst Date: Sun, 26 May 2024 22:23:40 +0200 Subject: [PATCH 12/12] Again --- db.sqlite3 | Bin 327680 -> 327680 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/db.sqlite3 b/db.sqlite3 index 8208a7551fe070ca2bd03d8d06cf998eb3d0238d..415b1cfbd34d07dde46bd12cba1bd1361a9d6ec0 100644 GIT binary patch delta 614 zcmah{Pe@a79Dl#}{kiAM&DVOe8m276!p!-5Z*OmVjz}Apm`;jH9SOSGrm4+l>Rib- z6{<@eztAlx>Xt*|-HMLgf+5wZbyQXhywt5W>C~a?hYz2Be6rbwY_?(ZWT!=L^1xYw zterZEW9vfaTDMhYU7F!m6lQqY`gX$HPLNaUe^FI2n0~)*kC=6P{I3D4EEK@HBR#1v z)KB8UZaB&dg?#S0z{g~q3qi~b^ue_ntJKR`IPQi;;EYA=>4PKqdJtX-bF`KV8EgR?6qz*k~gJ2)-Mk4qlaUJONJg*e!TWh=Tb9 zTt{gh4)fC2d@d^C!wifH4$2YYmvJWpi&pWw_+ES_X2fu<&Agd~7lLag>GP$MKEE-e zNAwwgDmFDWmsZkgCFZ-smc7%a@&HU9FFVZLw?JLQf#omIj8d`Uv%6)VUz6}<8O~Q2 zKg!@Jl1&so&^B7zCT!+&_kh*~SIclBIXk(~GPb%pCh!9rO%;_uI}ycyR21Fq*A(XUDn@l1Z2e5_ zX6h6DdFa0daiWozvHXob?LT*MaG)zboC?N+;gzA_Nbkbs<>6?2I?5(SMi;b1INIaC glG0`;BK?s|p{s-YIrI)@RNL&|rg{+bQHdty-@8e&^8f$< delta 590 zcmZo@5NT)-*`Th^CM0XjY|l8|K$}I9QFODrev<=_p_!F|xt^(oiG`tY^VR(ASMwRQ znqi`rrWWR=?cARkw{w4H`su?X#9__M^OAoS-(|k+=>iVSG8-HDIksDSFo&`7@TD-Y zF)*aEu`^hfPIvTRmYBZPmwCMazbgYf0|P@E3p;~#si>ml^uQY|;?uW!1Lf!XF$+zX z^@-Ok7GJ}0<-GI#yh;z6Y`lQw=3o`_cJmYP2ZT$9K-lu9+s&C+Shv60&&;C0$U6P~eP%hvSJQbOFiUI} zENJ4NzT!Qz8%V*2_sn7s7#pXvUtrN>VgJD1xUtcfT`i|Yj+4Pr&^DcslhMe)$VAt` zRM*H%!N|Z07^tSkdS>PZ7Dfis3oMyUrW=$ou}! ziA}eWnVx}(fq|u&2`Idzrgz?Dk(vJcCX43wiW@9Hgb4a=dZILw@bvd@Sr&RZd3)qp zIcEiWxu%(w8Rx`@280C}>n3^@8bp+31Xx5RmPa^y7wP(#Wx6{j6$OPtd}5|+U