From eeee72fbee807f889cacfdbdc20403d9ab1a5b2c Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Mon, 12 Sep 2022 18:57:44 +0530 Subject: [PATCH] multiprocessing final for visual lander --- .../visual_lander/web-template/brain.py | 166 +++++++++++ .../visual_lander/web-template/exercise.py | 266 ++++++------------ .../visual_lander/web-template/gui.py | 206 +++++++------- .../visual_lander/web-template/hal.py | 142 ++++++++-- .../web-template/launch/gazebo.launch | 24 -- .../web-template/launch/launch.py | 131 --------- .../web-template/launch/mavros.launch | 13 - .../web-template/launch/px4.launch | 18 -- .../web-template/launch/visual_lander.launch | 60 ++++ .../web-template/shared/__init__.py | 0 .../web-template/{ => shared}/car.py | 0 .../web-template/shared/image.py | 109 +++++++ .../web-template/shared/structure_img.py | 9 + .../web-template/shared/value.py | 93 ++++++ .../web-template/user_functions.py | 117 ++++++++ scripts/Dockerfile | 2 +- scripts/instructions.json | 5 +- 17 files changed, 869 insertions(+), 492 deletions(-) create mode 100755 exercises/static/exercises/visual_lander/web-template/brain.py mode change 100644 => 100755 exercises/static/exercises/visual_lander/web-template/exercise.py mode change 100644 => 100755 exercises/static/exercises/visual_lander/web-template/gui.py mode change 100644 => 100755 exercises/static/exercises/visual_lander/web-template/hal.py delete mode 100644 exercises/static/exercises/visual_lander/web-template/launch/gazebo.launch delete mode 100644 exercises/static/exercises/visual_lander/web-template/launch/launch.py delete mode 100644 exercises/static/exercises/visual_lander/web-template/launch/mavros.launch delete mode 100644 exercises/static/exercises/visual_lander/web-template/launch/px4.launch create mode 100755 exercises/static/exercises/visual_lander/web-template/launch/visual_lander.launch create mode 100755 exercises/static/exercises/visual_lander/web-template/shared/__init__.py rename exercises/static/exercises/visual_lander/web-template/{ => shared}/car.py (100%) create mode 100755 exercises/static/exercises/visual_lander/web-template/shared/image.py create mode 100755 exercises/static/exercises/visual_lander/web-template/shared/structure_img.py create mode 100755 exercises/static/exercises/visual_lander/web-template/shared/value.py create mode 100755 exercises/static/exercises/visual_lander/web-template/user_functions.py diff --git a/exercises/static/exercises/visual_lander/web-template/brain.py b/exercises/static/exercises/visual_lander/web-template/brain.py new file mode 100755 index 000000000..2330c04ba --- /dev/null +++ b/exercises/static/exercises/visual_lander/web-template/brain.py @@ -0,0 +1,166 @@ +import time +import threading +import multiprocessing +import sys +from datetime import datetime +import re +import json +import importlib + +import rospy +from std_srvs.srv import Empty +import cv2 + +from user_functions import GUIFunctions, HALFunctions +from console import start_console, close_console + +from shared.value import SharedValue + +# The brain process class +class BrainProcess(multiprocessing.Process): + def __init__(self, code, exit_signal): + super(BrainProcess, self).__init__() + + # Initialize exit signal + self.exit_signal = exit_signal + + # Function definitions for users to use + self.hal = HALFunctions() + self.gui = GUIFunctions() + + # Time variables + self.time_cycle = SharedValue('brain_time_cycle') + self.ideal_cycle = SharedValue('brain_ideal_cycle') + self.iteration_counter = 0 + + # Get the sequential and iterative code + # Something wrong over here! The code is reversing + # Found a solution but could not find the reason for this (parse_code function's return line of exercise.py is the reason) + self.sequential_code = code[1] + self.iterative_code = code[0] + + # Function to run to start the process + def run(self): + # Two threads for running and measuring + self.measure_thread = threading.Thread(target=self.measure_frequency) + self.thread = threading.Thread(target=self.process_code) + + self.measure_thread.start() + self.thread.start() + + print("Brain Process Started!") + + self.exit_signal.wait() + + # The process function + def process_code(self): + # Redirect information to console + start_console() + + # Reference Environment for the exec() function + iterative_code, sequential_code = self.iterative_code, self.sequential_code + + # The Python exec function + # Run the sequential part + gui_module, hal_module = self.generate_modules() + if sequential_code != "": + 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.exit_signal.is_set(): + start_time = datetime.now() + + # Execute the iterative portion + if iterative_code != "": + 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 it's more no problem as such, but we can change it! + time_cycle = self.time_cycle.get() + + if(ms < time_cycle): + time.sleep((time_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.get_frontal_image = self.hal.get_frontal_image + hal_module.HAL.get_ventral_image = self.hal.get_ventral_image + hal_module.HAL.takeoff = self.hal.takeoff + hal_module.HAL.land = self.hal.land + hal_module.HAL.set_cmd_pos = self.hal.set_cmd_pos + hal_module.HAL.set_cmd_vel = self.hal.set_cmd_vel + hal_module.HAL.set_cmd_mix = self.hal.set_cmd_mix + hal_module.HAL.get_position = self.hal.get_position + hal_module.HAL.get_velocity = self.hal.get_velocity + hal_module.HAL.get_orientation = self.hal.get_orientation + hal_module.HAL.get_roll = self.hal.get_roll + hal_module.HAL.get_pitch = self.hal.get_pitch + hal_module.HAL.get_yaw = self.hal.get_yaw + hal_module.HAL.get_landed_state = self.hal.get_landed_state + hal_module.HAL.get_yaw_rate = self.hal.get_yaw_rate + + + + # 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.showImage = self.gui.showImage + gui_module.GUI.showLeftImage = self.gui.showLeftImage + + # 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 not self.exit_signal.is_set(): + # 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.ideal_cycle.add(ms / self.iteration_counter) + except: + self.ideal_cycle.add(0) + + # Reset the counter + self.iteration_counter = 0 \ No newline at end of file diff --git a/exercises/static/exercises/visual_lander/web-template/exercise.py b/exercises/static/exercises/visual_lander/web-template/exercise.py old mode 100644 new mode 100755 index 3874651bc..82bf0c72a --- a/exercises/static/exercises/visual_lander/web-template/exercise.py +++ b/exercises/static/exercises/visual_lander/web-template/exercise.py @@ -1,3 +1,5 @@ + + #!/usr/bin/env python from __future__ import print_function @@ -5,6 +7,7 @@ from websocket_server import WebsocketServer import time import threading +import multiprocessing import subprocess import sys from datetime import datetime @@ -14,10 +17,14 @@ import rospy from std_srvs.srv import Empty +import cv2 + +from shared.value import SharedValue +from brain import BrainProcess +import queue + -from gui import GUI, ThreadGUI from hal import HAL -from car import Car from console import start_console, close_console @@ -26,37 +33,65 @@ class Template: # self.ideal_cycle to run an execution for at least 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 = True - self.user_code = "" + + self.brain_process = None + self.reload = multiprocessing.Event() # Time variables - self.ideal_cycle = 80 - self.measured_cycle = 80 - self.iteration_counter = 0 + self.brain_time_cycle = SharedValue('brain_time_cycle') + self.brain_ideal_cycle = SharedValue('brain_ideal_cycle') self.real_time_factor = 0 + self.frequency_message = {'brain': '', 'gui': '', 'rtf': ''} + # GUI variables + self.gui_time_cycle = SharedValue('gui_time_cycle') + self.gui_ideal_cycle = SharedValue('gui_ideal_cycle') + self.server = None self.client = None self.host = sys.argv[1] - # Initialize the GUI, HAL and Console behind the scenes self.hal = HAL() - self.car = Car() - self.gui = GUI(self.host, self.car) + self.paused = False + # 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): - sequential_code, iterative_code = self.seperate_seq_iter(source_code) - return iterative_code, sequential_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[6:]) + return iterative_code, sequential_code + + + # 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 separate the iterative and sequential code + # Function to seperate the iterative and sequential code def seperate_seq_iter(self, source_code): if source_code == "": return "", "" @@ -64,8 +99,8 @@ def seperate_seq_iter(self, source_code): # 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) - # Separate the content inside while True and the other - # (Separating the sequential and iterative part!) + # 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:] @@ -87,137 +122,16 @@ def seperate_seq_iter(self, source_code): return sequential_code, iterative_code - # The process function - def process_code(self, source_code): - # Redirect the information to console - start_console() - - iterative_code, sequential_code = self.parse_code(source_code) - - # 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} - while (self.stop_brain == True): - if (self.reload == True): - return - time.sleep(0.1) - exec(sequential_code, reference_environment) - - # Run the iterative part inside template - # and keep the check for flag - while self.reload == False: - while (self.stop_brain == True): - if (self.reload == True): - return - 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.drone = imp.new_module("drone") - # motors# hal_module.HAL.motors = imp.new_module("motors") - - # Add HAL functions - hal_module.HAL.get_frontal_image = self.hal.get_frontal_image - hal_module.HAL.get_ventral_image = self.hal.get_ventral_image - hal_module.HAL.get_position = self.hal.get_position - hal_module.HAL.get_velocity = self.hal.get_velocity - hal_module.HAL.get_yaw_rate = self.hal.get_yaw_rate - hal_module.HAL.get_orientation = self.hal.get_orientation - hal_module.HAL.get_roll = self.hal.get_roll - hal_module.HAL.get_pitch = self.hal.get_pitch - hal_module.HAL.get_yaw = self.hal.get_yaw - hal_module.HAL.get_landed_state = self.hal.get_landed_state - hal_module.HAL.set_cmd_pos = self.hal.set_cmd_pos - hal_module.HAL.set_cmd_vel = self.hal.set_cmd_vel - hal_module.HAL.set_cmd_mix = self.hal.set_cmd_mix - hal_module.HAL.takeoff = self.hal.takeoff - hal_module.HAL.land = self.hal.land - - # 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.showImage = self.gui.showImage - gui_module.GUI.showLeftImage = self.gui.showLeftImage - - # 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() - - # 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 + brain_frequency = 0;gui_frequency = 0 try: - brain_frequency = round(1000 / self.measured_cycle, 1) + brain_frequency = round(1000 / self.brain_ideal_cycle.get(), 1) except ZeroDivisionError: brain_frequency = 0 try: - gui_frequency = round(1000 / self.thread_gui.measured_cycle, 1) + gui_frequency = round(1000 / self.gui_ideal_cycle.get(), 1) except ZeroDivisionError: gui_frequency = 0 @@ -242,29 +156,32 @@ 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, universal_newlines=True) + stats_process = subprocess.Popen(args, stdout=subprocess.PIPE, bufsize=0) # 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, ''): - stats_list = [x.strip() for x in line.split(',')] - self.real_time_factor = stats_list[0] + 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 is not None: - while self.thread.is_alive(): - time.sleep(0.2) + if(self.brain_process != None): + while self.brain_process.is_alive(): + pass # Turn the flag down, the iteration has successfully stopped! - self.reload = False + self.reload.clear() # New thread execution - self.thread = threading.Thread(target=self.process_code, args=[source_code]) - self.thread.start() + code = self.parse_code(source_code) + if code[0] == "" and code[1] == "": + return + + self.brain_process = BrainProcess(code, self.reload) + self.brain_process.start() self.send_code_message() - print("New Thread Started!") # Function to read and set frequency from incoming message def read_frequency_message(self, message): @@ -272,65 +189,58 @@ def read_frequency_message(self, message): # Set brain frequency frequency = float(frequency_message["brain"]) - self.ideal_cycle = 1000.0 / frequency + self.brain_time_cycle.add(1000.0 / frequency) # Set gui frequency frequency = float(frequency_message["gui"]) - self.thread_gui.ideal_cycle = 1000.0 / frequency + self.gui_time_cycle.add(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": + 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"): + elif (message[:5] == "#code"): try: # Once received turn the reload flag up and send it to execute_thread function - self.user_code = message[6:] + code = message # print(repr(code)) - self.reload = True - self.execute_thread(self.user_code) + self.reload.set() + self.execute_thread(code) except: pass - elif (message[:5] == "#rest"): + elif (message[:5] == "#stop"): try: - self.reload = True - self.stop_brain = True - self.execute_thread(self.user_code) + self.reload.set() + self.execute_thread(code) except: pass - - elif (message[:5] == "#stop"): - self.stop_brain = True - - elif (message[:5] == "#play"): - self.stop_brain = False + self.server.send_message(self.client, "#stpd") # 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 HAL update thread + self.hal.start_thread() - # Start the real time factor tracker thread + # Start 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("After sneding freweq msg") print(client, 'connected') diff --git a/exercises/static/exercises/visual_lander/web-template/gui.py b/exercises/static/exercises/visual_lander/web-template/gui.py old mode 100644 new mode 100755 index 2973e6cb9..330a66cb0 --- a/exercises/static/exercises/visual_lander/web-template/gui.py +++ b/exercises/static/exercises/visual_lander/web-template/gui.py @@ -5,98 +5,76 @@ import time from datetime import datetime from websocket_server import WebsocketServer +import logging +import rospy +import cv2 +import sys +import numpy as np +import multiprocessing + +from interfaces.pose3d import ListenerPose3d +from shared.image import SharedImage +from shared.value import SharedValue +from shared.car import Car # Graphical User Interface Class class GUI: # Initialization function # The actual initialization def __init__(self, host, car): - t = threading.Thread(target=self.run_server) - + + rospy.init_node("GUI") self.payload = {'image': ''} self.left_payload = {'image': ''} self.server = None self.client = None - + self.host = host - # Image variables - self.image_to_be_shown = None - self.image_to_be_shown_updated = False - self.image_show_lock = threading.Lock() - - self.left_image_to_be_shown = None - self.left_image_to_be_shown_updated = False - self.left_image_show_lock = threading.Lock() - - self.acknowledge = False - self.acknowledge_lock = threading.Lock() - + # Image variable host + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Event objects for multiprocessing + self.ack_event = multiprocessing.Event() + self.cli_event = multiprocessing.Event() # Take the console object to set the same websocket and client self.car = car + + # Start server thread + t = threading.Thread(target=self.run_server) t.start() - # Explicit initialization function - # Class method, so user can call it without instantiation - @classmethod - def initGUI(cls, host): - # self.payload = {'image': '', 'shape': []} - new_instance = cls(host) - return new_instance # Function to prepare image payload # Encodes the image as a JSON string and sends through the WS def payloadImage(self): - self.image_show_lock.acquire() - image_to_be_shown_updated = self.image_to_be_shown_updated - image_to_be_shown = self.image_to_be_shown - self.image_show_lock.release() - - image = image_to_be_shown + image = self.shared_image.get() payload = {'image': '', 'shape': ''} - - if not image_to_be_shown_updated: - return payload - + shape = image.shape frame = cv2.imencode('.JPEG', image)[1] encoded_image = base64.b64encode(frame) - + payload['image'] = encoded_image.decode('utf-8') payload['shape'] = shape - - self.image_show_lock.acquire() - self.image_to_be_shown_updated = False - self.image_show_lock.release() - + return payload - + # Function to prepare image payload # Encodes the image as a JSON string and sends through the WS def payloadLeftImage(self): - self.left_image_show_lock.acquire() - left_image_to_be_shown_updated = self.left_image_to_be_shown_updated - left_image_to_be_shown = self.left_image_to_be_shown - self.left_image_show_lock.release() - - image = left_image_to_be_shown + image = self.shared_left_image.get() payload = {'image': '', 'shape': ''} - - if not left_image_to_be_shown_updated: - return payload - + shape = image.shape frame = cv2.imencode('.JPEG', image)[1] encoded_image = base64.b64encode(frame) - + payload['image'] = encoded_image.decode('utf-8') payload['shape'] = shape - - self.left_image_show_lock.acquire() - self.left_image_to_be_shown_updated = False - self.left_image_show_lock.release() - + return payload # Function for student to call @@ -117,27 +95,17 @@ def showLeftImage(self, image): # Called when a new client is received def get_client(self, client, server): self.client = client + self.cli_event.set() - # 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() - + print(client, 'connected') + + # Update the gui def update_gui(self): # Payload Image Message payload = self.payloadImage() self.payload["image"] = json.dumps(payload) - + message = "#gui" + json.dumps(self.payload) self.server.send_message(self.client, message) @@ -147,24 +115,34 @@ def update_gui(self): message = "#gul" + json.dumps(self.left_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) + + if(message[:4] == "#ack"): + # Set acknowledgement flag + self.ack_event.set() elif message[:4] == "#car": - self.car.start_car(int(message[4:5])) + self.car.start_car() elif message[:4] == "#stp": self.car.stop_car() + # Reset message elif message[:4] == "#rst": self.car.reset_car() + # Function that gets called when the connected closes + def handle_close(self, client, server): + print(client, 'closed') + + + # 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) + self.server.set_fn_client_left(self.handle_close) logged = False while not logged: @@ -181,36 +159,50 @@ def run_server(self): # Function to reset def reset_gui(self): pass - + # This class decouples the user thread # and the GUI update thread -class ThreadGUI: - def __init__(self, gui): - self.gui = gui +class ProcessGUI(multiprocessing.Process): + def __init__(self): + super(ProcessGUI, self).__init__() + self.host = sys.argv[1] + self.car = Car() # Time variables - self.ideal_cycle = 80 - self.measured_cycle = 80 + self.time_cycle = SharedValue("gui_time_cycle") + self.ideal_cycle = SharedValue("gui_ideal_cycle") self.iteration_counter = 0 + # Function to initialize events + def initialize_events(self): + # Events + self.ack_event = self.gui.ack_event + self.cli_event = self.gui.cli_event + self.exit_signal = multiprocessing.Event() + # Function to start the execution of threads - def start(self): + def run(self): + # Initialize GUI + self.gui = GUI(self.host, self.car) + self.initialize_events() + + # Wait for client before starting + self.cli_event.wait() self.measure_thread = threading.Thread(target=self.measure_thread) - self.thread = threading.Thread(target=self.run) + self.thread = threading.Thread(target=self.run_gui) self.measure_thread.start() self.thread.start() - print("GUI Thread Started!") + print("GUI Process Started!") + + self.exit_signal.wait() # The measuring thread to measure frequency def measure_thread(self): - while self.gui.client is None: - pass - previous_time = datetime.now() - while True: + while(True): # Sleep for 2 seconds time.sleep(2) @@ -223,32 +215,40 @@ def measure_thread(self): # Get the time period try: # Division by zero - self.measured_cycle = ms / self.iteration_counter + self.ideal_cycle.add(ms / self.iteration_counter) except: - self.measured_cycle = 0 + self.ideal_cycle.add(0) # Reset the counter self.iteration_counter = 0 # The main thread of execution - def run(self): - while self.gui.client is None: - pass - - while True: + def run_gui(self): + while(True): start_time = datetime.now() + # Send update signal self.gui.update_gui() - acknowledge_message = self.gui.get_acknowledge() - - while not acknowledge_message: - acknowledge_message = self.gui.get_acknowledge() - - self.gui.set_acknowledge(False) + # Wait for acknowldege signal + self.ack_event.wait() + self.ack_event.clear() + 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) + time_cycle = self.time_cycle.get() + + if(ms < time_cycle): + time.sleep((time_cycle-ms) / 1000.0) + + self.exit_signal.set() + + # Functions to handle auxillary GUI functions + def reset_gui(self): + self.gui.reset_gui() + +if __name__ == "__main__": + gui = ProcessGUI() + gui.start() \ No newline at end of file diff --git a/exercises/static/exercises/visual_lander/web-template/hal.py b/exercises/static/exercises/visual_lander/web-template/hal.py old mode 100644 new mode 100755 index 25635a119..17c39cd6b --- a/exercises/static/exercises/visual_lander/web-template/hal.py +++ b/exercises/static/exercises/visual_lander/web-template/hal.py @@ -1,9 +1,12 @@ -import numpy as np import rospy import cv2 +import threading +import time +from datetime import datetime from drone_wrapper import DroneWrapper - +from shared.image import SharedImage +from shared.value import SharedValue # Hardware Abstraction Layer class HAL: @@ -12,71 +15,166 @@ class HAL: def __init__(self): rospy.init_node("HAL") - + + self.shared_frontal_image = SharedImage("halfrontalimage") + self.shared_ventral_image = SharedImage("halventralimage") + self.shared_x = SharedValue("x") + self.shared_y = SharedValue("y") + self.shared_z = SharedValue("z") + self.shared_takeoff_z = SharedValue("sharedtakeoffz") + self.shared_az = SharedValue("az") + self.shared_azt = SharedValue("azt") + self.shared_vx = SharedValue("vx") + self.shared_vy = SharedValue("vy") + self.shared_vz = SharedValue("vz") + self.shared_landed_state = SharedValue("landedstate") + self.shared_position = SharedValue("position") + self.shared_velocity = SharedValue("velocity") + self.shared_orientation = SharedValue("orientation") + self.shared_roll = SharedValue("roll") + self.shared_pitch = SharedValue("pitch") + self.shared_yaw = SharedValue("yaw") + self.shared_yaw_rate = SharedValue("yawrate") + self.image = None - self.drone = DroneWrapper(name="rqt") + self.drone = DroneWrapper(name="rqt",ns="/iris/") + + # Update thread + self.thread = ThreadHAL(self.update_hal) # Explicit initialization functions # Class method, so user can call it without instantiation - @classmethod - def initRobot(cls): - new_instance = cls() - return new_instance + + + # Function to start the update thread + def start_thread(self): + self.thread.start() # Get Image from ROS Driver Camera def get_frontal_image(self): image = self.drone.get_frontal_image() image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - return image_rgb + self.shared_frontal_image.add(image_rgb) def get_ventral_image(self): image = self.drone.get_ventral_image() image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - return image_rgb + self.shared_ventral_image.add(image_rgb) def get_position(self): pos = self.drone.get_position() - return pos + self.shared_position.add(pos,type_name="list") def get_velocity(self): vel = self.drone.get_velocity() - return vel + self.shared_velocity.add(vel ,type_name="list") def get_yaw_rate(self): yaw_rate = self.drone.get_yaw_rate() - return yaw_rate + self.shared_yaw_rate.add(yaw_rate) def get_orientation(self): orientation = self.drone.get_orientation() - return orientation + self.shared_orientation.add(orientation ,type_name="list") def get_roll(self): roll = self.drone.get_roll() - return roll + self.shared_roll.add(roll) def get_pitch(self): pitch = self.drone.get_pitch() - return pitch + self.shared_pitch.add(pitch) def get_yaw(self): yaw = self.drone.get_yaw() - return yaw + self.shared_yaw.add(yaw) def get_landed_state(self): state = self.drone.get_landed_state() - return state + self.shared_landed_state.add(state) + + def set_cmd_pos(self): + x = self.shared_x.get() + y = self.shared_y.get() + z = self.shared_z.get() + az = self.shared_az.get() - def set_cmd_pos(self, x, y, z, az): self.drone.set_cmd_pos(x, y, z, az) - def set_cmd_vel(self, vx, vy, vz, az): + def set_cmd_vel(self): + vx = self.shared_vx.get() + vy = self.shared_vy.get() + vz = self.shared_vz.get() + az = self.shared_azt.get() self.drone.set_cmd_vel(vx, vy, vz, az) - def set_cmd_mix(self, vx, vy, z, az): + def set_cmd_mix(self): + vx = self.shared_vx.get() + vy = self.shared_vy.get() + z = self.shared_z.get() + az = self.shared_azt.get() self.drone.set_cmd_mix(vx, vy, z, az) - def takeoff(self, h=3): + def takeoff(self): + h = self.shared_takeoff_z.get() self.drone.takeoff(h) def land(self): self.drone.land() + + def update_hal(self): + self.get_frontal_image() + self.get_ventral_image() + self.get_position() + self.get_velocity() + self.get_yaw_rate() + self.get_orientation() + self.get_pitch() + self.get_roll() + self.get_yaw() + self.get_landed_state() + self.set_cmd_pos() + self.set_cmd_vel() + self.set_cmd_mix() + + # Destructor function to close all fds + def __del__(self): + self.shared_frontal_image.close() + self.shared_ventral_image.close() + self.shared_x.close() + self.shared_y.close() + self.shared_z.close() + self.shared_takeoff_z.close() + self.shared_az.close() + self.shared_azt.close() + self.shared_vx.close() + self.shared_vy.close() + self.shared_vz.close() + self.shared_landed_state.close() + self.shared_position.close() + self.shared_velocity.close() + self.shared_orientation.close() + self.shared_roll.close() + self.shared_pitch.close() + self.shared_yaw.close() + self.shared_yaw_rate.close() + +class ThreadHAL(threading.Thread): + def __init__(self, update_function): + super(ThreadHAL, self).__init__() + self.time_cycle = 80 + self.update_function = update_function + + def run(self): + while(True): + start_time = datetime.now() + + self.update_function() + + 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.time_cycle): + time.sleep((self.time_cycle - ms) / 1000.0) \ No newline at end of file diff --git a/exercises/static/exercises/visual_lander/web-template/launch/gazebo.launch b/exercises/static/exercises/visual_lander/web-template/launch/gazebo.launch deleted file mode 100644 index 7768d03f8..000000000 --- a/exercises/static/exercises/visual_lander/web-template/launch/gazebo.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/exercises/static/exercises/visual_lander/web-template/launch/launch.py b/exercises/static/exercises/visual_lander/web-template/launch/launch.py deleted file mode 100644 index d22afda8e..000000000 --- a/exercises/static/exercises/visual_lander/web-template/launch/launch.py +++ /dev/null @@ -1,131 +0,0 @@ -#!/usr/bin/env python3 - -import stat -import rospy -from os import lstat -from subprocess import Popen, PIPE - - -DRI_PATH = "/dev/dri/card0" -EXERCISE = "visual_lander" -TIMEOUT = 30 -MAX_ATTEMPT = 2 - - -# Check if acceleration can be enabled -def check_device(device_path): - try: - return stat.S_ISCHR(lstat(device_path)[stat.ST_MODE]) - except: - return False - - -# Spawn new process -def spawn_process(args, insert_vglrun=False): - if insert_vglrun: - args.insert(0, "vglrun") - process = Popen(args, stdout=PIPE, bufsize=1, universal_newlines=True) - return process - - -class Test(): - def gazebo(self): - rospy.logwarn("[GAZEBO] Launching") - try: - rospy.wait_for_service("/gazebo/get_model_properties", TIMEOUT) - return True - except rospy.ROSException: - return False - - def px4(self): - rospy.logwarn("[PX4-SITL] Launching") - start_time = rospy.get_time() - args = ["./PX4-Autopilot/build/px4_sitl_default/bin/px4-commander","--instance", "0", "check"] - while rospy.get_time() - start_time < TIMEOUT: - process = spawn_process(args, insert_vglrun=False) - with process.stdout: - for line in iter(process.stdout.readline, ''): - if ("Prearm check: OK" in line): - return True - rospy.sleep(2) - return False - - def mavros(self, ns=""): - rospy.logwarn("[MAVROS] Launching") - try: - rospy.wait_for_service(ns + "/mavros/cmd/arming", TIMEOUT) - return True - except rospy.ROSException: - return False - - -class Launch(): - def __init__(self): - self.test = Test() - self.acceleration_enabled = check_device(DRI_PATH) - - # Start roscore - args = ["/opt/ros/noetic/bin/roscore"] - spawn_process(args, insert_vglrun=False) - - rospy.init_node("launch", anonymous=True) - - def start(self): - ######## LAUNCH GAZEBO ######## - args = ["/opt/ros/noetic/bin/roslaunch", - "/RoboticsAcademy/exercises/" + EXERCISE + "/web-template/launch/gazebo.launch", - "--wait", - "--log" - ] - - attempt = 1 - while True: - spawn_process(args, insert_vglrun=self.acceleration_enabled) - if self.test.gazebo() == True: - break - if attempt == MAX_ATTEMPT: - rospy.logerr("[GAZEBO] Launch Failed") - return - attempt = attempt + 1 - - - ######## LAUNCH PX4 ######## - args = ["/opt/ros/noetic/bin/roslaunch", - "/RoboticsAcademy/exercises/" + EXERCISE + "/web-template/launch/px4.launch", - "--log" - ] - - attempt = 1 - while True: - spawn_process(args, insert_vglrun=self.acceleration_enabled) - if self.test.px4() == True: - break - if attempt == MAX_ATTEMPT: - rospy.logerr("[PX4] Launch Failed") - return - attempt = attempt + 1 - - - ######## LAUNCH MAVROS ######## - args = ["/opt/ros/noetic/bin/roslaunch", - "/RoboticsAcademy/exercises/" + EXERCISE + "/web-template/launch/mavros.launch", - "--log" - ] - - attempt = 1 - while True: - spawn_process(args, insert_vglrun=self.acceleration_enabled) - if self.test.mavros() == True: - break - if attempt == MAX_ATTEMPT: - rospy.logerr("[MAVROS] Launch Failed") - return - attempt = attempt + 1 - - -if __name__ == "__main__": - launch = Launch() - launch.start() - - with open("/drones_launch.log", "w") as f: - f.write("success") diff --git a/exercises/static/exercises/visual_lander/web-template/launch/mavros.launch b/exercises/static/exercises/visual_lander/web-template/launch/mavros.launch deleted file mode 100644 index b899c0ec1..000000000 --- a/exercises/static/exercises/visual_lander/web-template/launch/mavros.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/exercises/static/exercises/visual_lander/web-template/launch/px4.launch b/exercises/static/exercises/visual_lander/web-template/launch/px4.launch deleted file mode 100644 index 43f1f66a9..000000000 --- a/exercises/static/exercises/visual_lander/web-template/launch/px4.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/exercises/static/exercises/visual_lander/web-template/launch/visual_lander.launch b/exercises/static/exercises/visual_lander/web-template/launch/visual_lander.launch new file mode 100755 index 000000000..9b4962255 --- /dev/null +++ b/exercises/static/exercises/visual_lander/web-template/launch/visual_lander.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/exercises/static/exercises/visual_lander/web-template/shared/__init__.py b/exercises/static/exercises/visual_lander/web-template/shared/__init__.py new file mode 100755 index 000000000..e69de29bb diff --git a/exercises/static/exercises/visual_lander/web-template/car.py b/exercises/static/exercises/visual_lander/web-template/shared/car.py similarity index 100% rename from exercises/static/exercises/visual_lander/web-template/car.py rename to exercises/static/exercises/visual_lander/web-template/shared/car.py diff --git a/exercises/static/exercises/visual_lander/web-template/shared/image.py b/exercises/static/exercises/visual_lander/web-template/shared/image.py new file mode 100755 index 000000000..de5c9f9d6 --- /dev/null +++ b/exercises/static/exercises/visual_lander/web-template/shared/image.py @@ -0,0 +1,109 @@ +import numpy as np +import mmap +from posix_ipc import Semaphore, O_CREX, ExistentialError, O_CREAT, SharedMemory, unlink_shared_memory +from ctypes import sizeof, memmove, addressof, create_string_buffer +from shared.structure_img import MD + +# Probably, using self variables gives errors with memmove +# Therefore, a global variable for utility +md_buf = create_string_buffer(sizeof(MD)) + +class SharedImage: + def __init__(self, name): + # Initialize variables for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.md_buf = None; self.md_region = None + self.image_lock = None + + self.shm_name = name; self.md_name = name + "-meta" + self.image_lock_name = name + + # Initialize or retreive metadata memory region + try: + self.md_region = SharedMemory(self.md_name) + self.md_buf = mmap.mmap(self.md_region.fd, sizeof(MD)) + self.md_region.close_fd() + except ExistentialError: + self.md_region = SharedMemory(self.md_name, O_CREAT, size=sizeof(MD)) + self.md_buf = mmap.mmap(self.md_region.fd, self.md_region.size) + self.md_region.close_fd() + + # Initialize or retreive Semaphore + try: + self.image_lock = Semaphore(self.image_lock_name, O_CREX) + except ExistentialError: + image_lock = Semaphore(self.image_lock_name, O_CREAT) + image_lock.unlink() + self.image_lock = Semaphore(self.image_lock_name, O_CREX) + + self.image_lock.release() + + # Get the shared image + def get(self): + # Define metadata + metadata = MD() + + # Get metadata from the shared region + self.image_lock.acquire() + md_buf[:] = self.md_buf + memmove(addressof(metadata), md_buf, sizeof(metadata)) + self.image_lock.release() + + # Try to retreive the image from shm_buffer + # Otherwise return a zero image + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, metadata.size) + self.shm_region.close_fd() + + self.image_lock.acquire() + image = np.ndarray(shape=(metadata.shape_0, metadata.shape_1, metadata.shape_2), + dtype='uint8', buffer=self.shm_buf) + self.image_lock.release() + + # Check for a None image + if(image.size == 0): + image = np.zeros((3, 3, 3), np.uint8) + + except ExistentialError: + image = np.zeros((3, 3, 3), np.uint8) + + return image + + # Add the shared image + def add(self, image): + try: + # Get byte size of the image + byte_size = image.nbytes + + # Get the shared memory buffer to read from + if not self.shm_region: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + + # Generate meta data + metadata = MD(image.shape[0], image.shape[1], image.shape[2], byte_size) + + # Send the meta data and image to shared regions + self.image_lock.acquire() + memmove(md_buf, addressof(metadata), sizeof(metadata)) + self.md_buf[:] = md_buf[:] + self.shm_buf[:] = image.tobytes() + self.image_lock.release() + except: + pass + + # Destructor function to unlink and disconnect + def close(self): + self.image_lock.acquire() + self.md_buf.close() + + try: + unlink_shared_memory(self.md_name) + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.image_lock.release() + self.image_lock.close() diff --git a/exercises/static/exercises/visual_lander/web-template/shared/structure_img.py b/exercises/static/exercises/visual_lander/web-template/shared/structure_img.py new file mode 100755 index 000000000..ae40b3707 --- /dev/null +++ b/exercises/static/exercises/visual_lander/web-template/shared/structure_img.py @@ -0,0 +1,9 @@ +from ctypes import Structure, c_int32, c_int64 + +class MD(Structure): + _fields_ = [ + ('shape_0', c_int32), + ('shape_1', c_int32), + ('shape_2', c_int32), + ('size', c_int64) + ] \ No newline at end of file diff --git a/exercises/static/exercises/visual_lander/web-template/shared/value.py b/exercises/static/exercises/visual_lander/web-template/shared/value.py new file mode 100755 index 000000000..e7bfad8a2 --- /dev/null +++ b/exercises/static/exercises/visual_lander/web-template/shared/value.py @@ -0,0 +1,93 @@ +import numpy as np +import mmap +from posix_ipc import Semaphore, O_CREX, ExistentialError, O_CREAT, SharedMemory, unlink_shared_memory +from ctypes import sizeof, memmove, addressof, create_string_buffer, c_float +import struct + +class SharedValue: + def __init__(self, name): + # Initialize varaibles for memory regions and buffers and Semaphore + self.shm_buf = None; self.shm_region = None + self.value_lock = None + + self.shm_name = name; self.value_lock_name = name + + # Initialize or retreive Semaphore + try: + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + except ExistentialError: + value_lock = Semaphore(self.value_lock_name, O_CREAT) + value_lock.unlink() + self.value_lock = Semaphore(self.value_lock_name, O_CREX) + + self.value_lock.release() + + # Get the shared value + def get(self, type_name= "value"): + # Retreive the data from buffer + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + self.value_lock.acquire() + value = struct.unpack('f', self.shm_buf)[0] + self.value_lock.release() + + return value + elif type_name=="list": + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + self.value_lock.acquire() + array_val = np.ndarray(shape=(3,), + dtype='float32', buffer=self.shm_buf) + self.value_lock.release() + + return array_val + + else: + print("missing argument for return type") + + + # Add the shared value + def add(self, value, type_name= "value"): + # Send the data to shared regions + if type_name=="value": + try: + self.shm_region = SharedMemory(self.shm_name) + self.shm_buf = mmap.mmap(self.shm_region.fd, sizeof(c_float)) + self.shm_region.close_fd() + except ExistentialError: + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=sizeof(c_float)) + self.shm_buf = mmap.mmap(self.shm_region.fd, self.shm_region.size) + self.shm_region.close_fd() + + self.value_lock.acquire() + self.shm_buf[:] = struct.pack('f', value) + self.value_lock.release() + elif type_name=="list": + byte_size = value.nbytes + self.shm_region = SharedMemory(self.shm_name, O_CREAT, size=byte_size) + self.shm_buf = mmap.mmap(self.shm_region.fd, byte_size) + self.shm_region.close_fd() + self.value_lock.acquire() + self.shm_buf[:] = value.tobytes() + self.value_lock.release() + + # Destructor function to unlink and disconnect + def close(self): + self.value_lock.acquire() + self.shm_buf.close() + + try: + unlink_shared_memory(self.shm_name) + except ExistentialError: + pass + + self.value_lock.release() + self.value_lock.close() diff --git a/exercises/static/exercises/visual_lander/web-template/user_functions.py b/exercises/static/exercises/visual_lander/web-template/user_functions.py new file mode 100755 index 000000000..d741601fc --- /dev/null +++ b/exercises/static/exercises/visual_lander/web-template/user_functions.py @@ -0,0 +1,117 @@ +from shared.image import SharedImage +from shared.value import SharedValue +import numpy as np +import cv2 + +# Define HAL functions +class HALFunctions: + def __init__(self): + # Initialize image variable + self.shared_frontal_image = SharedImage("halfrontalimage") + self.shared_ventral_image = SharedImage("halventralimage") + self.shared_x = SharedValue("x") + self.shared_y = SharedValue("y") + self.shared_z = SharedValue("z") + self.shared_takeoff_z = SharedValue("sharedtakeoffz") + self.shared_az = SharedValue("az") + self.shared_azt = SharedValue("azt") + self.shared_vx = SharedValue("vx") + self.shared_vy = SharedValue("vy") + self.shared_vz = SharedValue("vz") + self.shared_landed_state = SharedValue("landedstate") + self.shared_position = SharedValue("position") + self.shared_velocity = SharedValue("velocity") + self.shared_orientation = SharedValue("orientation") + self.shared_roll = SharedValue("roll") + self.shared_pitch = SharedValue("pitch") + self.shared_yaw = SharedValue("yaw") + self.shared_yaw_rate = SharedValue("yawrate") + + + # Get image function + def get_frontal_image(self): + image = self.shared_frontal_image.get() + return image + + # Get left image function + def get_ventral_image(self): + image = self.shared_ventral_image.get() + return image + + def takeoff(self, height): + self.shared_takeoff_z.add(height) + + def land(self): + pass + + def set_cmd_pos(self, x, y , z, az): + self.shared_x.add(x) + self.shared_y.add(y) + self.shared_z.add(z) + self.shared_az.add(az) + + def set_cmd_vel(self, vx, vy, vz, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(vz) + self.shared_azt.add(az) + + def set_cmd_mix(self, vx, vy, z, az): + self.shared_vx.add(vx) + self.shared_vy.add(vy) + self.shared_vz.add(z) + self.shared_azt.add(az) + + + def get_position(self): + position = self.shared_position.get(type_name = "list") + return position + + def get_velocity(self): + velocity = self.shared_velocity.get(type_name = "list") + return velocity + + def get_yaw_rate(self): + yaw_rate = self.shared_yaw_rate.get(type_name = "value") + return yaw_rate + + def get_orientation(self): + orientation = self.shared_orientation.get(type_name = "list") + return orientation + + def get_roll(self): + roll = self.shared_roll.get(type_name = "value") + return roll + + def get_pitch(self): + pitch = self.shared_pitch.get(type_name = "value") + return pitch + + def get_yaw(self): + yaw = self.shared_yaw.get(type_name = "value") + return yaw + + def get_landed_state(self): + landed_state = self.shared_landed_state.get(type_name = "value") + return landed_state + +# Define GUI functions +class GUIFunctions: + def __init__(self): + # Initialize image variable + self.shared_image = SharedImage("guifrontalimage") + self.shared_left_image = SharedImage("guiventralimage") + + # Show image function + def showImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_image.add(image) + + # Show left image function + def showLeftImage(self, image): + # Reshape to 3 channel if it has only 1 in order to display it + if (len(image.shape) < 3): + image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + self.shared_left_image.add(image) \ No newline at end of file diff --git a/scripts/Dockerfile b/scripts/Dockerfile index ef3c72c4e..1365ec1de 100644 --- a/scripts/Dockerfile +++ b/scripts/Dockerfile @@ -60,4 +60,4 @@ EXPOSE 1831 ENTRYPOINT [ "./entrypoint.sh" ] -# CMD ["--help"] \ No newline at end of file +# CMD ["--help"] diff --git a/scripts/instructions.json b/scripts/instructions.json index 1aa963c34..ca49b1a93 100644 --- a/scripts/instructions.json +++ b/scripts/instructions.json @@ -89,8 +89,9 @@ }, "visual_lander": { "gazebo_path": "/RoboticsAcademy/exercises/visual_lander/web-template/launch", - "instructions_ros": ["python3 ./RoboticsAcademy/exercises/visual_lander/web-template/launch/launch.py"], - "instructions_host": "python3 /RoboticsAcademy/exercises/visual_lander/web-template/exercise.py 0.0.0.0" + "instructions_ros": ["/opt/ros/noetic/bin/roslaunch ./RoboticsAcademy/exercises/visual_lander/web-template/launch/visual_lander.launch"], + "instructions_host": "python3 /RoboticsAcademy/exercises/visual_lander/web-template/exercise.py 0.0.0.0", + "instructions_gui": "python3 /RoboticsAcademy/exercises/visual_lander/web-template/gui.py 0.0.0.0 {}" }, "opticalflow_teleop": { "gazebo_path": "/RoboticsAcademy/exercises/opticalflow_teleop/web-template/launch",