class AUV():
"""AUV Master, automates tasks"""
def __init__(self):
rospy.init_node('AUV', anonymous=True) # initialize AUV rosnode
rospy.Subscriber(
'kill_switch', Int8,
self.kill_switch_callback) # Subscriber for magnet kill switch
self.motor_state = None
self.tasks = None
# self.test
self.config = Config() # initialize Config() class
self.read_config()
self.motor = Motor(self.motor_state) # initialize Motor() class
self.navigation = Navigation() # initialize Navigation() class
self.keyboard = Keyboard(
self.navigation) # initialize Keyboard() class
self.status_logger = StatusLogger() # initialize StatusLogger() class
# try:
self.houston = Houston(self.navigation,
self.tasks) # initialize Houston() class
# except NameError:
# print('Houston is not initialized.')
def kill_switch_callback(self, data):
if data.data == 1:
self.start()
if data.data == 0:
self.stop()
def read_config(self):
""" Reads from config/config.ini"""
self.motor_state = self.config.get_config(
'auv', 'motor_state') # read motor state from config
self.tasks = self.config.get_config('auv',
'tasks') # read tasks from config
def keyboard_nav(self):
"""Navigate the robosub using keyboard controls"""
self.keyboard.getch()
def perform_tasks(self):
"""Has houston perform task"""
# try:
self.houston.start_all_tasks()
# except AttributeError:
# print('houston not initialized')
def specific_task(self, task_num):
"""Has houston do specific task"""
self.houston.do_one_task(task_num)
def stop_task(self):
"""Has houston stop task"""
self.houston.stop_task()
def display_tasks(self):
"""Has houston display list of tasks"""
self.houston.print_tasks()
# def stop_one_task(self, task_num):
# self.houston.stop_one_task(task_num)
def start(self):
"""Starts the modules when magnet killswitch is plugged in"""
self.motor.start()
self.navigation.start()
self.keyboard.start()
self.status_logger.start()
# try:
self.houston.start()
# except AttributeError:
# print('houston not initialized')
# self.cv.start(self.tasks)
def stop(self):
"""Stops the modules when magnet killswitch is removed"""
self.motor.stop()
self.navigation.stop()
self.keyboard.stop()
self.status_logger.stop()
# try:
self.houston.stop()
class AUV():
"""AUV Master, automates tasks"""
def __init__(self):
rospy.init_node('AUV', anonymous=True) # initialize AUV rosnode
rospy.Subscriber(
'kill_switch', Int8,
self.kill_switch_callback) # Subscriber for magnet kill switch
self.motor_state = None
self.tasks = None
# self.config = Config() # initialize Config() class
self.read_config() # read parameters from the config.ini file
self.motor = Motor(self.motor_state) # initialize Motor() class
self.navigation = Navigation() # initialize Navigation() class
self.keyboard = Keyboard(
self.navigation) # initialize Keyboard() class
self.houston = Houston(self.navigation,
self.tasks) # initialize Houston() class
def kill_switch_callback(self, data):
if data.data == 1:
self.start()
if data.data == 0:
self.stop()
def open_config(self):
""" Opens the config file and updates the parameters"""
os.system('gedit config/config.ini')
self.update_config()
def update_config(self):
""" Loads the updated parameters from config"""
self.read_config()
self.houston.cvcontroller.set_model(
) # read and set all models from config
def update_color(self):
""" Update RGB/HSV for computer vision from config"""
lower_color = config.get_config('cv', 'lower_color')
upper_color = config.get_config('cv', 'upper_color')
self.houston.cvcontroller.set_lower_color(lower_color[0],
lower_color[1:])
self.houston.cvcontroller.set_upper_color(upper_color[0],
upper_color[1:])
def read_config(self):
""" Reads from config/config.ini"""
self.motor_state = config.get_config(
'auv', 'motor_state') # read motor state from config
self.tasks = config.get_config('auv',
'tasks') # read tasks from config
def keyboard_nav(self):
"""Navigate the robosub using keyboard controls"""
self.keyboard.getch()
def perform_tasks(self):
"""Has houston perform task"""
self.houston.start_task('all', None)
def specific_task(self, task_num):
"""Has houston do specific task"""
self.houston.start_task('one', task_num)
def specific_task_for_gui(self, task_name):
"""Has houston do specific task from gui"""
self.houston.start_task_from_gui('one', task_name)
def stop_task(self):
"""Has houston stop task"""
self.houston.stop_task()
def display_tasks(self):
"""Has houston display list of tasks"""
self.houston.print_tasks()
# def stop_one_task(self, task_num):
# self.houston.stop_one_task(task_num)
def start(self):
"""Starts the modules when magnet killswitch is plugged in"""
self.motor.start()
self.navigation.start()
self.keyboard.start()
self.houston.start()
def stop(self):
"""Stops the modules when magnet killswitch is removed"""
self.motor.stop()
self.navigation.stop()
self.keyboard.stop()
status.stop()
self.houston.stop()
def save_heading(self):
self.navigation.save_current_heading()
class AUV():
"""AUV Master, automates tasks"""
def __init__(self):
rospy.init_node('AUV', anonymous=True) # initialize AUV rosnode
rospy.Subscriber(
'kill_switch', Int8,
self.kill_switch_callback) # Subscriber for magnet kill switch
self.motor_state = 0
self.tasks = []
self.get_config()
# self.test
self.motor = Motor(self.motor_state) # initialize Motor() class
self.navigation = Navigation() # initialize Navigation() class
self.keyboard = Keyboard() # initialize Keyboard() class
self.status_logger = StatusLogger() # initialize StatusLogger() class
# TODO self.cv = CV() # initialize CV() class
def kill_switch_callback(self, data):
if data.data == 1:
self.start()
if data.data == 0:
self.stop()
def get_config(self):
"""Reads variables from config/config.ini file.
Creates config.ini file from template_config.ini file if config.ini does not exist
"""
config = ConfigParser.RawConfigParser()
config_file_path = 'config/config.ini'
try:
config.readfp(open(config_file_path))
except IOError:
print('setting up config.ini file.')
copyfile('config/template_config.ini', 'config/config.ini')
config.readfp(open(config_file_path))
if config.has_option('auv_config', 'motor_state'):
self.motor_state = config.getint('auv_config', 'motor_state')
print('motor state: %d' % self.motor_state)
if config.has_option('auv_config', 'tasks'):
self.tasks = config.get('auv_config', 'tasks').split(', ')
if not self.tasks and config.has_option('auv_config',
'default_tasks'):
self.tasks = config.get('auv_config',
'default_tasks').split(', ')
print('tasks: %s' % self.tasks)
def set_config(self, var, value, is_reset=False):
"""Assign given value to respective var in config/config.ini or reset to default values"""
config = ConfigParser.RawConfigParser()
config_file_path = 'config/config.ini'
try:
config.readfp(open(config_file_path))
except IOError:
print('setting up config.ini file.')
copyfile('config/template_config.ini', 'config/config.ini')
config.readfp(open(config_file_path))
if var == 'tasks' and is_reset:
config.set('auv_config', 'tasks',
config.get('auv_config', 'default_tasks'))
self.tasks = config.get('auv_config', 'default_tasks').split()
print('tasks: %s' % self.tasks)
elif var == 'tasks' and not is_reset:
# TODO set tasks
value.split()
with open(config_file_path, 'wb') as configfile:
config.write(configfile)
def keyboard_nav(self):
"""Navigate the robosub using keyboard controls"""
self.keyboard.getch()
def start(self):
"""Starts the modules when magnet killswitch is plugged in"""
self.motor.start()
self.navigation.start()
self.keyboard.start()
# self.cv.start(self.tasks)
def stop(self):
"""Stops the modules when magnet killswitch is removed"""
self.motor.stop()
self.navigation.stop()
self.keyboard.stop()
class Keyboard():
"""Navigate the robosub using keyboard controls
w: forwards
a: counter-clockwise
s: backwards
d: clockwise
q: left
e: right
r: up
f: down
[0-9]: power [1]: 10% [0]: 100%
`: stop
c: custom power
v: custom rotation
h: set height
x: exit
"""
def __init__(self, navigation=None):
self.is_killswitch_on = False
self.multiplier = 40
self.r_multiplier = 18.0
# self.waypoint = Waypoint()
# self.navigation = Navigation(self.waypoint)
if navigation:
self.navigation = navigation
else:
self.navigation = Navigation()
self.h_power = 100
self.m_power = 100
self.r_power = 100
self.thread_w = None
self.exit = False
self.w_time_delay = 20
def getch(self):
"""Gets keyboard input if killswitch is plugged in"""
getch = _Getch()
accepted = ['w', 'a', 's', 'd', 'q', 'e', 'r', 'f', '`']
response = ''
char = 0
rotation = self.r_multiplier
height = 0.0
if self.is_killswitch_on:
print('\
\nw: forwards\
\na: counter-clockwise\
\ns: backwards\
\nd: clockwise\
\nq: left\
\ne: right\
\nr: up\
\nf: down\
\n[0-9]: power [1]: 10% [0]: 100%\
\n`: stop\
\nm: custom movement power\
\n,: custom height power\
\n.: custom rotation power\
\nv: custom rotation\
\nh: set height\
\ng: record waypoint\
\nt: go to last waypoint\
\no: set all waypoint start delay\
\np: run through all waypoints\
\nx: exit')
while char != 'x':
char = getch()
if char in accepted:
self.navigate(char, rotation, height)
elif char.isdigit():
if char == '0':
self.m_power = int(10) * self.multiplier
self.r_power = self.m_power
rotation = int(10) * self.r_multiplier
else:
self.m_power = int(char) * self.multiplier
self.r_power = self.m_power
rotation = int(char) * self.r_multiplier
print('power: %d rotation: %.2f degrees' %
(self.m_power, rotation))
elif char == '.':
while not response.isdigit() or int(response) < 0 or int(
response) > 400:
response = raw_input(
'\nEnter a custom rotation power value [0-400]: ')
self.r_power = int(response)
response = ''
print('rotation power: %d' % self.r_power)
elif char == ',':
while not response.isdigit() or int(response) < 0 or int(
response) > 400:
response = raw_input(
'\nEnter a custom height power value [0-400]: ')
self.h_power = int(response)
response = ''
print('height power: %d' % self.h_power)
elif char == 'm':
while not response.isdigit() or int(response) < 0 or int(
response) > 400:
response = raw_input(
'\nEnter a custom movement power value [0-400]: ')
self.m_power = int(response)
response = ''
print('movement power: %d' % self.m_power)
elif char == 'o':
while not response.isdigit() or int(response) < 0 or int(
response) > 400:
response = raw_input(
'\nEnter a time delay in seconds: ')
self.w_time_delay = int(response)
response = ''
print('delay time: %d' % self.w_time_delay)
elif char == 'v':
while True:
try:
response = raw_input(
'\nEnter a custom rotation value [0-180]: ')
rotation = float(response)
if rotation < 0.0 or rotation > 180.0:
raise ValueError
except ValueError:
pass
else:
break
response = ''
print('rotation: %.2f' % rotation)
elif char == 'h':
while True:
try:
response = raw_input('\nEnter a height: ')
height = float(response)
except ValueError:
pass
else:
break
response = ''
print('height: %.2f' % height)
elif char == 'g':
self.navigation.push_current_waypoint()
elif char == 't':
self.navigation.run_top_stack_waypoint(
self.r_power, self.h_power, self.m_power)
elif char == 'p':
print('waiting %d seconds' % self.w_time_delay)
time.sleep(self.w_time_delay)
self.navigation.run_stack_waypoints_async()
self.navigation.set_exit_waypoints(True)
else:
print('Magnet is not plugged in.')
def navigate(self, char, rotation, height):
"""Navigates robosub with given character input and power"""
if char == '`':
self.navigation.cancel_h_nav(self.h_power)
self.navigation.cancel_r_nav(self.r_power)
self.navigation.cancel_m_nav(self.m_power)
elif char == 'w':
self.navigation.cancel_m_nav()
self.navigation.m_nav('power', 'forward', self.m_power)
# self.navigation.m_nav('distance', 'forward', self.m_power, 1)
elif char == 'a':
self.navigation.cancel_r_nav()
self.navigation.r_nav('left', rotation, self.r_power)
elif char == 's':
self.navigation.cancel_m_nav()
self.navigation.m_nav('power', 'backward', self.m_power)
# self.navigation.m_nav('distance', 'backward', self.m_power, 1)
elif char == 'd':
self.navigation.cancel_r_nav()
self.navigation.r_nav('right', rotation, self.r_power)
elif char == 'q':
self.navigation.cancel_m_nav()
self.navigation.m_nav('power', 'left', self.m_power)
# self.navigation.m_nav('distance', 'left', self.m_power, 1)
elif char == 'e':
self.navigation.cancel_m_nav()
self.navigation.m_nav('power', 'right', self.m_power)
# self.navigation.m_nav('distance', 'right', self.m_power, 1)
elif char == 'r':
self.navigation.cancel_h_nav(self.h_power)
self.navigation.h_nav('up', height, self.h_power)
elif char == 'f':
self.navigation.cancel_h_nav(self.h_power)
self.navigation.h_nav('down', height, self.h_power)
def set_power(self, **power_type):
"""
Sets the power to a value set_power(power_type=value)
power_type: h_power, m_power, r_power
"""
if 'h_power' in power_type:
self.h_power = power_type['h_power']
if 'm_power' in power_type:
self.h_power = power_type['m_power']
if 'r_power' in power_type:
self.h_power = power_type['r_power']
def start(self):
"""Allows keyboard navigation when killswitch is plugged in"""
self.is_killswitch_on = True
self.navigation.start()
def stop(self):
"""Stops keyboard navigation when killswitch is unplugged"""
self.is_killswitch_on = False
self.navigation.stop()
class Keyboard():
"""Navigate the robosub using keyboard controls
w: forwards
a: counter-clockwise
s: backwards
d: clockwise
q: left
e: right
r: up
f: down
[0-9]: power [1]: 10% [0]: 100%
`: stop
c: custom power
v: custom rotation
h: set height
x: exit
"""
def __init__(self):
self.is_killswitch_on = False
self.multiplier = 40
self.r_multiplier = 18.0
self.navigation = Navigation()
def getch(self):
"""Gets keyboard input if killswitch is plugged in"""
getch = _Getch()
accepted = ['w', 'a', 's', 'd', 'q', 'e', 'r', 'f', '`']
response = ''
char = 0
power = self.multiplier
rotation = self.r_multiplier
height = 0.0
if self.is_killswitch_on:
print(
'\
\nw: forwards\
\na: counter-clockwise\
\ns: backwards\
\nd: clockwise\
\nq: left\
\ne: right\
\nr: up\
\nf: down\
\n[0-9]: power [1]: 10% [0]: 100%\
\n`: stop\
\nc: custom power\
\nv: custom rotation\
\nh: set height\
\nx: exit')
while char != 'x':
char = getch()
if char in accepted:
self.navigate(char, power, rotation, height)
elif char.isdigit():
if char == '0':
power = int(10) * self.multiplier
rotation = int(10) * self.r_multiplier
else:
power = int(char) * self.multiplier
rotation = int(char) * self.r_multiplier
print('power: %d rotation: %.2f degrees' % (power, rotation))
elif char == 'c':
while not response.isdigit() or int(response) < 0 or int(response) > 400:
response = raw_input('\nEnter a custom power value [0-400]: ')
power = int(response)
response = ''
print('power: %d' % power)
elif char == 'v':
while True:
try:
response = raw_input('\nEnter a custom rotation value [0-180]: ')
rotation = float(response)
if rotation < 0.0 or rotation > 180.0:
raise ValueError
except ValueError:
pass
else:
break
response = ''
print('rotation: %.2f' % rotation)
elif char == 'h':
while True:
try:
response = raw_input('\nEnter a height: ')
height = float(response)
except ValueError:
pass
else:
break
response = ''
print('height: %.2f' % height)
else:
print('Magnet is not plugged in.')
def navigate(self, char, power, rotation, height):
"""Navigates robosub with given character input and power"""
if char == '`':
self.navigation.h_nav('staying', 0, 0)
self.navigation.r_nav('staying', 0, 0)
self.navigation.m_nav('power', 'none', 0)
elif char == 'w':
self.navigation.m_nav('power', 'forward', power)
elif char == 'a':
self.navigation.r_nav('left', rotation, power)
elif char == 's':
self.navigation.m_nav('power', 'backward', power)
elif char == 'd':
self.navigation.r_nav('right', rotation, power)
elif char == 'q':
self.navigation.m_nav('power', 'left', power)
elif char == 'e':
self.navigation.m_nav('power', 'right', power)
elif char == 'r':
self.navigation.h_nav('up', height, power)
elif char == 'f':
self.navigation.h_nav('down', height, power)
def start(self):
"""Allows keyboard navigation when killswitch is plugged in"""
self.is_killswitch_on = True
self.navigation.start()
def stop(self):
"""Stops keyboard navigation when killswitch is unplugged"""
self.is_killswitch_on = False
self.navigation.stop()
class Keyboard():
"""Navigate the robosub using keyboard controls
w: forwards
a: counter-clockwise
s: backwards
d: clockwise
q: left
e: right
r: up
f: down
[0-9]: power
c: custom power
x: exit
"""
def __init__(self):
self.is_killswitch_on = False
self.multiplier = 40
self.navigation = Navigation()
def getch(self):
"""Gets keyboard input if killswitch is plugged in"""
getch = _Getch()
accepted = ['w', 'a', 's', 'd', 'q', 'e', 'r', 'f']
response = ''
char = 0
power = self.multiplier
if self.is_killswitch_on:
print('\
\nw: forwards\
\na: counter-clockwise\
\ns: backwards\
\nd: clockwise\
\nq: left\
\ne: right\
\nr: up\
\nf: down\
\n[0-9]: power\
\nc: custom power\
\nx: exit')
while char != 'x':
char = getch()
if char in accepted:
self.navigate(char, power)
elif char.isdigit():
if char == '0':
power = int(10) * self.multiplier
else:
power = int(char) * self.multiplier
print('power is changed to %s' % power)
elif char == 'c':
while not response.isdigit() or int(response) < 0 or int(
response) > 400:
response = raw_input(
'\nEnter a custom power value [0-400]: ')
power = response
response = ''
print('power is changed to %s' % power)
else:
print('Magnet is not plugged in.')
def navigate(self, char, power):
"""Navigates robosub with given character input and power"""
if char == 'w':
self.navigation.navigate(0, power, 0, 0.0)
elif char == 'a':
self.navigation.navigate(0, 0, 0, -10.0)
elif char == 's':
self.navigation.navigate(0, -power, 0, 0.0)
elif char == 'd':
self.navigation.navigate(0, 0, 0, 10.0)
elif char == 'q':
self.navigation.navigate(-power, 0, 0, 0.0)
elif char == 'e':
self.navigation.navigate(power, 0, 0, 0.0)
elif char == 'r':
self.navigation.navigate(0, 0, power, 0.0)
elif char == 'f':
self.navigation.navigate(0, 0, -power, 0.0)
def start(self):
"""Allows keyboard navigation when killswitch is plugged in"""
self.is_killswitch_on = True
self.navigation.start()
def stop(self):
"""Stops keyboard navigation when killswitch is unplugged"""
self.is_killswitch_on = False
self.navigation.stop()