def __init__(self, host):
rospy.init_node("GUI")
self.payload = {'image': '','lap': '', 'map': '', 'v':'','w':''}
self.server = None
self.client = None
self.host = host
# Image variable
self.shared_image = SharedImage("guiimage")
# Get HAL variables
self.shared_v = SharedValue("velocity")
self.shared_w = SharedValue("angular")
# Create the lap object
pose3d_object = ListenerPose3d("/F1ROS/odom")
self.lap = Lap(pose3d_object)
self.map = Map(pose3d_object)
# Event objects for multiprocessing
self.ack_event = multiprocessing.Event()
self.cli_event = multiprocessing.Event()
# Start server thread
t = threading.Thread(target=self.run_server)
t.start()
def __init__(self):
super(ProcessGUI, self).__init__()
self.host = sys.argv[1]
# Time variables
self.time_cycle = SharedValue("gui_time_cycle")
self.ideal_cycle = SharedValue("gui_ideal_cycle")
self.iteration_counter = 0
def __init__(self):
rospy.init_node("HAL")
# Shared memory variables
self.shared_image = SharedImage("halimage")
self.shared_v = SharedValue("velocity")
self.shared_w = SharedValue("angular")
# ROS Topics
self.camera = ListenerCamera("/F1ROS/cameraL/image_raw")
self.motors = PublisherMotors("/F1ROS/cmd_vel", 4, 0.3)
# Update thread
self.thread = ThreadHAL(self.update_hal)
class HAL:
IMG_WIDTH = 320
IMG_HEIGHT = 240
def __init__(self):
rospy.init_node("HAL")
# Shared memory variables
self.shared_image = SharedImage("halimage")
self.shared_v = SharedValue("velocity")
self.shared_w = SharedValue("angular")
# ROS Topics
self.camera = ListenerCamera("/F1ROS/cameraL/image_raw")
self.motors = PublisherMotors("/F1ROS/cmd_vel", 4, 0.3)
# Update thread
self.thread = ThreadHAL(self.update_hal)
# Function to start the update thread
def start_thread(self):
self.thread.start()
# Get Image from ROS Driver Camera
def getImage(self):
image = self.camera.getImage().data
self.shared_image.add(image)
# Set the velocity
def setV(self):
velocity = self.shared_v.get()
self.motors.sendV(velocity)
# Get the velocity
def getV(self):
velocity = self.shared_v.get()
return velocity
# Get the angular velocity
def getW(self):
angular = self.shared_w.get()
return angular
# Set the angular velocity
def setW(self):
angular = self.shared_w.get()
self.motors.sendW(angular)
def update_hal(self):
self.getImage()
self.setV()
self.setW()
# Destructor function to close all fds
def __del__(self):
self.shared_image.close()
self.shared_v.close()
self.shared_w.close()
class HALFunctions:
def __init__(self):
# Initialize image variable
self.shared_image = SharedImage("halimage")
self.shared_v = SharedValue("velocity")
self.shared_w = SharedValue("angular")
# Get image function
def getImage(self):
image = self.shared_image.get()
return image
# Send velocity function
def sendV(self, velocity):
self.shared_v.add(velocity)
# Send angular velocity function
def sendW(self, angular):
self.shared_w.add(angular)
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
self.sequential_code = code[1]
self.iterative_code = code[0]
def __init__(self):
self.brain_process = None
self.reload = multiprocessing.Event()
# Time variables
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 and HAL behind the scenes
self.hal = HAL()
class Template:
# Initialize class variables
# self.time_cycle to run an execution for atleast 1 second
# self.process for the current running process
def __init__(self):
self.brain_process = None
self.reload = multiprocessing.Event()
# Time variables
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 and HAL behind the scenes
self.hal = HAL()
# 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 "", ""
elif(source_code[:5] == "#resu"):
restart_simulation = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
restart_simulation()
return "", ""
elif(source_code[:5] == "#paus"):
pause_simulation = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
pause_simulation()
return "", ""
elif(source_code[:5] == "#rest"):
reset_simulation = rospy.ServiceProxy('/gazebo/reset_world', Empty)
reset_simulation()
return "", ""
else:
sequential_code, iterative_code = self.seperate_seq_iter(source_code)
return iterative_code, sequential_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'[^ \t]while\(True\):|[^ \t]while True:', 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\(True\):|[^ ]while True:', '', iterative_code)
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 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.brain_process != None):
while self.brain_process.is_alive():
pass
# Turn the flag down, the iteration has successfully stopped!
self.reload.clear()
# New thread execution
code = self.parse_code(source_code)
if code[0] == "" and code[1] == "":
return
self.brain_process = BrainProcess(code, self.reload)
self.brain_process.start()
# 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.brain_time_cycle.add(1000.0 / frequency)
# Set gui frequency
frequency = float(frequency_message["gui"])
self.gui_time_cycle.add(1000.0 / frequency)
return
# 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)
# 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(',')]
self.real_time_factor = stats_list[0]
# 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.brain_ideal_cycle.get(), 1)
except ZeroDivisionError:
brain_frequency = 0
try:
gui_frequency = round(1000 / self.gui_ideal_cycle.get(), 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)
# 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)
self.send_frequency_message()
return
try:
# Once received turn the reload flag up and send it to execute_thread function
code = message
# print(repr(code))
self.reload.set()
self.execute_thread(code)
except:
pass
# Function that gets called when the server is connected
def connected(self, client, server):
self.client = client
# Start the HAL update thread
self.hal.start_thread()
# Start real time factor tracker thread
self.stats_thread = threading.Thread(target=self.track_stats)
self.stats_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)
self.server.run_forever()
def __init__(self):
# Initialize image variable
self.shared_image = SharedImage("halimage")
self.shared_v = SharedValue("velocity")
self.shared_w = SharedValue("angular")
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
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
# print(sequential_code)
# print(iterative_code)
# Whatever the code is, first step is to just stop!
self.hal.sendV(0)
self.hal.sendW(0)
# 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 = imp.new_module("HAL")
hal_module.HAL = imp.new_module("HAL")
hal_module.HAL.motors = imp.new_module("motors")
# Add HAL functions
hal_module.HAL.getImage = self.hal.getImage
hal_module.HAL.motors.sendV = self.hal.sendV
hal_module.HAL.motors.sendW = self.hal.sendW
# Define GUI module
gui_module = imp.new_module("GUI")
gui_module.GUI = imp.new_module("GUI")
# 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 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
class GUI:
# Initialization function
# The actual initialization
def __init__(self, host):
rospy.init_node("GUI")
self.payload = {'image': '','lap': '', 'map': '', 'v':'','w':''}
self.server = None
self.client = None
self.host = host
# Image variable
self.shared_image = SharedImage("guiimage")
# Get HAL variables
self.shared_v = SharedValue("velocity")
self.shared_w = SharedValue("angular")
# Create the lap object
pose3d_object = ListenerPose3d("/F1ROS/odom")
self.lap = Lap(pose3d_object)
self.map = Map(pose3d_object)
# Event objects for multiprocessing
self.ack_event = multiprocessing.Event()
self.cli_event = multiprocessing.Event()
# Start server thread
t = threading.Thread(target=self.run_server)
t.start()
# Function to prepare image payload
# Encodes the image as a JSON string and sends through the WS
def payloadImage(self):
image = self.shared_image.get()
payload = {'image': '', 'shape': ''}
shape = image.shape
frame = cv2.imencode('.JPEG', image)[1]
encoded_image = base64.b64encode(frame)
payload['image'] = encoded_image.decode('utf-8')
payload['shape'] = shape
return payload
# Function to get the client
# Called when a new client is received
def get_client(self, client, server):
self.client = client
self.cli_event.set()
print(client, 'connected')
# Update the gui
def update_gui(self):
# Payload Image Message
payload = self.payloadImage()
self.payload["image"] = json.dumps(payload)
# Payload Lap Message
lapped = self.lap.check_threshold()
self.payload["lap"] = ""
if(lapped != None):
self.payload["lap"] = str(lapped)
# Payload Map Message
pos_message = str(self.map.getFormulaCoordinates())
self.payload["map"] = pos_message
# Payload V Message
v_message = str(self.shared_v.get())
self.payload["v"] = v_message
# Payload W Message
w_message = str(self.shared_w.get())
self.payload["w"] = w_message
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"):
# Set acknowledgement flag
self.ack_event.set()
# Pause message
elif(message[:5] == "#paus"):
self.lap.pause()
# Unpause message
elif(message[:5] == "#resu"):
self.lap.unpause()
# Reset message
elif(message[:5] == "#rest"):
self.reset_gui()
# 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)
self.server.run_forever()
# Function to reset
def reset_gui(self):
self.lap.reset()
self.map.reset()
class ProcessGUI(multiprocessing.Process):
def __init__(self):
super(ProcessGUI, self).__init__()
self.host = sys.argv[1]
# Time variables
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 run(self):
# Initialize GUI
self.gui = GUI(self.host)
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_gui)
self.measure_thread.start()
self.thread.start()
print("GUI Process Started!")
self.exit_signal.wait()
# The measuring thread to measure frequency
def measure_thread(self):
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.ideal_cycle.add(ms / self.iteration_counter)
except:
self.ideal_cycle.add(0)
# Reset the counter
self.iteration_counter = 0
# The main thread of execution
def run_gui(self):
while(True):
start_time = datetime.now()
# Send update signal
self.gui.update_gui()
# 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
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.gui.reset_gui()
def lap_pause():
self.gui.lap.pause()
def lap_unpause():
self.gui.lap.unpause()