diff --git a/db.sqlite3 b/db.sqlite3 index f7dc38f9e..415b1cfbd 100644 Binary files a/db.sqlite3 and b/db.sqlite3 differ 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/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): 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/3d_reconstruction_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/3d_reconstruction_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/ros1_noetic/interfaces/threadPublisher.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros1_noetic/interfaces/threadPublisher.py deleted file mode 100644 index 69aa0ad48..000000000 --- a/exercises/static/exercises/vacuum_cleaner_loc_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/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..7c690772c --- /dev/null +++ b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/GUI.py @@ -0,0 +1,210 @@ +import json +import cv2 +import math +import base64 +import threading +import time +from datetime import datetime +import websocket +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 + +# 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): + + # ROS2 init + rclpy.init(args=None) + node = rclpy.create_node('GUI') + + self.payload = {'map': '', 'user': ''} + self.server = None + self.client = None + self.init_coords = (171, 63) + self.start_coords = (201, 85.5) + + self.host = host + + self.ack = False + self.ack_lock = threading.Lock() + + self.shared_image = SharedImage("guiimage") + + 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): + pos_message = self.start_coords + ang_message = self.map.getRobotAngle() + 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: + 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) + + # 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) + + 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_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_interface.showNumpy(image) + +def getMap(self, url): + 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/HAL.py b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/python_template/ros2_humble/HAL.py new file mode 100755 index 000000000..5900922fd --- /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") +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(float(v)) + + +# Angular speed +def setW(w): + motor_node.sendW(float(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/react-components/SpecificVacuumCleaner.js b/exercises/static/exercises/vacuum_cleaner_loc_newmanager/react-components/SpecificVacuumCleaner.js index 008eeac21..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 @@ -2,28 +2,33 @@ 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] - ); + 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)); - // Send the ACK of the img - window.RoboticsExerciseComponents.commsManager.send("gui", "ack"); + draw( + guiCanvasRef.current, + content[0], + content[1], + content[2], + content[3], + userContent[0], + userContent[1], + userContent[2], + userContent[3] + ); + } }; window.RoboticsExerciseComponents.commsManager.subscribe( @@ -40,12 +45,35 @@ function SpecificVacuumCleaner(props) { }; }, []); + 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 (