def set_models(self):
if self.name in thruster_models:
tags = thruster_models[self.name]
self.curve_forward, self.curve_reverse = \
[self.parse_model("%s/%s.polynomial" % (MODEL_DIR, tag)) for tag in tags]
self.curve_reverse = [-x for x in self.curve_reverse]
else:
log("No model for %s thruster, defaulting to VideoRay!" % self.name)
def set_models(self):
if self.name in thruster_models:
tags = thruster_models[self.name]
self.curve_forward, self.curve_reverse = \
[self.parse_model("%s/%s.polynomial" % (MODEL_DIR, tag)) for tag in tags]
self.curve_reverse = [-x for x in self.curve_reverse]
else:
log("No model for %s thruster, defaulting to VideoRay!" %
self.name)
# If the name given is a class or function, instantiate it and pass arguments.
if isinstance(task, (type, types.FunctionType, functools.partial)):
converted_args = []
for arg in args.args:
try:
f_arg = float(arg)
except ValueError:
f_arg = arg
converted_args.append(f_arg)
task = task(*converted_args)
elif len(args.args) > 0:
log("Arguments only supported for uninstantiated Tasks.",
copy_to_stdout=True)
sys.exit(1)
def release_lock():
os.rmdir(lock_dir)
initially_killed = shm.switches.hard_kill.get()
was_ever_unkilled = False
def cleanup():
Zero()()
release_lock()
shm.vision_modules.set(vision_state)
def loop(kalman_watcher, quit_event):
# PID stepping class
pid = PIDLoop(speed=args['speed'])
pid.clean()
# Optimizer class
opt = Optimizer()
opt.DEBUG = bool(args['verbose'])
controller_enabled = True
tm = ThrusterManager()
kalman_watcher.watch(kalman)
# The main loop
while not quit_event.is_set():
if not args['rate']:
kalman_watcher.wait()
start_time = time.time()
g = kalman.get()
dt_qs = timed(tm.update, g)[1]
# Get passive forces and passive torques on the sub in model frame
passives, dt_pass = timed(vehicle.passive_forces, g, tm)
set_shm_wrench(control_passive_forces, passives)
# Handle controller enable / disable feature
if not settings_control.enabled.get() or \
switches.soft_kill.get() or switches.hard_kill.get():
# controller is disabled
if controller_enabled:
set_shm_wrench(control_internal_wrench, [0] * 6)
zero_motors()
controller_enabled = False
if args['rate']:
time.sleep(0.1)
continue
if not controller_enabled: # We have been re-enabled
controller_enabled = True
pid.clean()
# Execute one PID step for all PID controllers.
dt_pid = timed(pid.step)[1]
# Set motors from PID desires.
dt_opt = timed(opt.set_motors, tm, passives)[1]
total_time = time.time() - start_time
if args['verbose']:
times = [("Qs", dt_qs), ("Passives", dt_pass),
("PID", dt_pid), ("Optimizer", dt_opt), ("Total", total_time)]
for name, dt in times:
log("%s step ran in %0.2f HZ (%f ms)" % (name, (1/dt), 1000*dt), copy_to_stdout=True)
if args['rate']:
if total_time > step_time:
log(str(datetime.now()) + \
"## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time), copy_to_stdout=True)
else:
if args['verbose']:
log("Rate capped at %f HZ" % args['rate'], copy_to_stdout=True)
log("-----", copy_to_stdout=True) #visual break
time.sleep(step_time - total_time)
pid.clean()
zero_motors()
"""
Logging Example!
"""
from auvlog.client import Logger, log
# A happy message to all!
log('Today is a good day!', copy_to_stdout=True)
# A secret message!
log.mymodule.secret({'answer': 43.12, 'havingFun': True}, copy_to_stdout=True)
'''
# Another!
log.fun.secret('The question, however...')
# Yet another!
log.really.fun.secret('... is unknown.')
# A complicated message!
log.mission.abstractcombinator.vectoredio.convolution(
'Critical failure: code 92.')
# A realistic message!
def myfunc():
log.vision.main.default('Vision system initialized.')
myfunc()
log = log.mission.main
all_thrusters = []
for thruster in vehicle.thrusters:
def get_default(key, default):
return thruster[key] if key in thruster else default
h, p = thruster['heading_pitch']
reversed_polarity = get_default('reversed', False)
broken = get_default('broken', False)
t_class_s = get_default('type', 'VideoRay')
vector = get_default('vector', False)
if t_class_s not in globals():
log("Invalid type \"%s\" for thruster %s!" %
(t_class_s, thruster['name']),
copy_to_stdout=True)
raise Exception()
t_class = globals()[t_class_s]
min_pos_pwm = get_default('min_pos_pwm', t_class.min_pwm)
min_neg_pwm = get_default('min_neg_pwm', -t_class.min_pwm)
thruster_name = thruster.get('real_name', thruster['name'])
thruster = t_class(yaw=h,
pitch=p,
position=thruster['pos'],
link=thruster['name'],
name=thruster_name,
vector=vector,
def loop(kalman_watcher, quit_event):
# PID stepping class
pid = PIDLoop(speed=args['speed'])
pid.clean()
# Optimizer class
opt = Optimizer()
opt.DEBUG = bool(args['verbose'])
controller_enabled = True
tm = ThrusterManager()
kalman_watcher.watch(shm.kalman)
# The main loop
while not quit_event.is_set():
if not args['rate']:
kalman_watcher.wait()
start_time = time.time()
g = shm.kalman.get()
dt_qs = timed(tm.update, g)[1]
# Get passive forces and passive torques on the sub in model frame
passives, dt_pass = timed(vehicle.passive_forces, g, tm)
set_shm_wrench(shm.control_passive_forces, passives)
# Handle controller enable / disable feature
if not shm.settings_control.enabled.get() or \
shm.switches.soft_kill.get() or shm.switches.hard_kill.get():
# controller is disabled
if controller_enabled:
set_shm_wrench(shm.control_internal_wrench, [0] * 6)
zero_motors()
controller_enabled = False
if args['rate']:
time.sleep(0.1)
continue
if not controller_enabled: # We have been re-enabled
controller_enabled = True
pid.clean()
# Execute one PID step for all PID controllers.
dt_pid = timed(pid.step, tm)[1]
# Set motors from PID desires.
dt_opt = timed(opt.set_motors, tm, passives)[1]
total_time = time.time() - start_time
if args['verbose']:
times = [("Qs", dt_qs), ("Passives", dt_pass), ("PID", dt_pid),
("Optimizer", dt_opt), ("Total", total_time)]
for name, dt in times:
log("%s step ran in %0.2f HZ (%f ms)" %
(name, (1 / dt), 1000 * dt),
copy_to_stdout=True)
if args['rate']:
if total_time > step_time:
log(str(datetime.now()) + \
"## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time), copy_to_stdout=True)
else:
if args['verbose']:
log("Rate capped at %f HZ" % args['rate'],
copy_to_stdout=True)
log("-----", copy_to_stdout=True) #visual break
time.sleep(step_time - total_time)
pid.clean()
zero_motors()
# If the name given is a class or function, instantiate it and pass arguments.
if isinstance(task, (type, types.FunctionType, functools.partial)):
converted_args = []
for arg in args.args:
try:
f_arg = float(arg)
except ValueError:
f_arg = arg
converted_args.append(f_arg)
task = task(*converted_args)
elif len(args.args) > 0:
log("Arguments only supported for uninstantiated Tasks.",
copy_to_stdout=True)
sys.exit(1)
period = 1 / args.frequency
logger = log.mission.main
# Save running vision module state
vision_state = shm.vision_modules.get()
logger('Saved running vision module state. Will restore on Ctrl-C or mission completion.', copy_to_stdout = True)
# Ensure only one mission can run at a time.
LOCK_NAME = ".mission_lock"
lock_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), LOCK_NAME)
try:
"""
Logging Example!
"""
from auvlog.client import Logger, log
# A happy message to all!
log('Today is a good day!')
# A secret message!
log.secret('The answer is 43.12')
# Another!
log.fun.secret('The question, however...')
# Yet another!
log.really.fun.secret('... is unknown.')
# A complicated message!
log.mission.abstractcombinator.vectoredio.convolution(
'Critical failure: code 92.')
# A realistic message!
def myfunc():
log.vision.main.default('Vision system initialized.')
myfunc()
log = log.mission.main
"""
Logging Example!
"""
from auvlog.client import Logger, log
# A happy message to all!
log('Today is a good day!', copy_to_stdout = True)
# A secret message!
log.mymodule.secret({'answer': 43.12, 'havingFun': True}, copy_to_stdout = True)
'''
# Another!
log.fun.secret('The question, however...')
# Yet another!
log.really.fun.secret('... is unknown.')
# A complicated message!
log.mission.abstractcombinator.vectoredio.convolution(
'Critical failure: code 92.')
# A realistic message!
def myfunc():
log.vision.main.default('Vision system initialized.')
myfunc()
# List of all thrusters on the vehicle (including possibly broken thrusters)
# Used for thruster tests
all_thrusters = []
for thruster in vehicle.thrusters:
def get_default(key, default):
return thruster[key] if key in thruster else default
h, p = thruster['heading_pitch']
reversed_polarity = get_default('reversed', False)
broken = get_default('broken', False)
t_class_s = get_default('type', 'VideoRay')
if t_class_s not in globals():
log("Invalid type \"%s\" for thruster %s!" % (t_class_s, thruster['name']),
copy_to_stdout=True)
raise Exception()
t_class = globals()[t_class_s]
thruster_name = thruster.get('real_name', thruster['name'])
thruster = t_class(yaw=h, pitch=p, position=thruster['pos'],
link=thruster['name'], name=thruster_name,
reversed_polarity=reversed_polarity, broken=broken)
all_thrusters.append(thruster)
# Thrusters contains an array of all non-broken thrusters
# to be used for control
thrusters = [x for x in all_thrusters if not x.broken]
