Position Control multiprocessing

exercises/static/exercises/position_control/web-template/brain.py

@@ -0,0 +1,169 @@
+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
+        hal_module.HAL.init_beacons = self.hal.init_beacons
+        hal_module.HAL.get_next_beacon = self.hal.get_next_beacon
+
+
+
+
+        # 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