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]