print("{}, but another type was assumed. This is most likely a bug with auv-shn-cli.".format(e)) print(" {}.{} is currently {}".format(args.group, args.variable, variable.get())) if args.watch: def f(watcher, quit_event): watcher.watch(group) string = " {}.{} is currently {{}}".format(args.group, args.variable) while True: watcher.wait(new_update=False) if quit_event.is_set(): break print(string.format(variable.get())) watch_thread_wrapper(f) # No variable given, display all variables in the group else: if args.reset: with open(os.path.join(os.getenv('CUAUV_SOFTWARE'), 'libshm/c/shm.c')) as f: for line in f: if "shm_set({}".format(args.group) in line: variable = line.split("shm_set({}, ".format(args.group))[1].split(")")[0].split(", ") call(["auv-shm-cli", args.group] + variable) else: print_group(args.group, group) if args.watch: def f(watcher, quit_event): watcher.watch(group)
#!/usr/bin/env python2 """ Starts a new shmlog every time the vehicle enters the water. Alex Spitzer 2014 """ import subprocess import shm from misc.utils import watch_thread_wrapper LOG_CMD = "auv-shmlogd" def uptime_watcher(w, quit_event): w.watch(shm.uptime) log_process = None while not quit_event.is_set(): in_water = shm.uptime.in_water.get() if in_water and log_process is None: log_process = subprocess.Popen(LOG_CMD) elif not in_water and log_process is not None: log_process.terminate() log_process = None w.wait(False) watch_thread_wrapper(uptime_watcher)
observations = [] localizer = Localizer(shm.hydrophones_settings.track_frequency_target.get()) while not quit_event.is_set(): #input() results = shm.hydrophones_results_track.get() kalman = shm.kalman.get() sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) print("------------------------------------------------------") localizer.add_observation((results.diff_phase_x, results.diff_phase_y), sub_pos, sub_quat) pinger_pos, pinger_pos_all = localizer.compute_position() print("%d observations. current position: %0.2f %0.2f %0.2f" % \ (len(localizer.observations), sub_pos[0], sub_pos[1], sub_pos[2])) print("Predicted (2 Trans): %0.2f %0.2f %0.2f" % \ (pinger_pos[0], pinger_pos[1], pinger_pos[2])) print("Predicted (3 Trans): %0.2f %0.2f %0.2f" % \ (pinger_pos_all[0], pinger_pos_all[1], pinger_pos_all[2])) watcher.wait(new_update=False) shm.hydrophones_settings.track_frequency_target.set(PINGER_FREQ) shm.hydrophones_settings.track_magnitude_threshold.set(TRACK_MAG_THRESH) watch_thread_wrapper(func)
for name, (x, y), prob in zip(shapes, positions, shape_out_probs)] if __name__ == "__main__": def daemon(w, quit_event): print "== Bins Machine Learning Classifier Daemon ==" print "Loading SVM data..." c = Classifier() print "SVM data loaded!" w.watch(shm.shape_results_4) w.watch(shm.shape_settings) while 1: w.wait(True) if quit_event.is_set(): break bins_set = [] if shm.shape_settings.enabled: for (name, (x, y, prob)) in c.classify(): bins_set.append(name) set_shm_for_key(name, x, y, prob) for name in set(bin_options.keys()) - set(bins_set): set_shm_for_key(name, 0, 0, 0.0) watch_thread_wrapper(daemon)
def main(args): if args['rate']: step_time = 1.0 / args['rate'] log("Starting controller at %f HZ, one step every %f seconds" % ( args['rate'], step_time), copy_to_stdout=True) else: log("Starting controller with kalman lock", copy_to_stdout=True) 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() watch_thread_wrapper(loop)
print(newvec) print(dangle(newvec, oldvec)) if dangle(newvec, oldvec) > MAX_DANGLE: print("diff too big") continue oldvec = newvec o = shm.hydrophones_filtered.get() o.count = g.ping_count o.period = g.ping_time o.intensity = g.intensity o.elevation = g.elevation PAST_HEADINGS[PAST_HEADINGS_I] = resvector() s = np.zeros((2)) for i in range(0, PAST_HEADINGS_N): s += PAST_HEADINGS[i] o.heading = (math.atan2(s[0], s[1]) * 360 / (2 * math.pi) + 360) % 360 PAST_HEADINGS_I = (PAST_HEADINGS_I + 1) % PAST_HEADINGS_N o.uuid = random.randint(-sys.maxsize - 1, sys.maxsize) print('send heading: {0}'.format(o.heading)) shm.hydrophones_filtered.set(o) random.seed() watch_thread_wrapper(loop)
print(oldvec) print(newvec) print(dangle(newvec, oldvec)) if dangle(newvec, oldvec) > MAX_DANGLE: print("diff too big") continue oldvec = newvec o = shm.hydrophones_filtered.get() o.count = g.ping_count o.period = g.ping_time o.intensity = g.intensity o.elevation = g.elevation PAST_HEADINGS[PAST_HEADINGS_I] = resvector() s = np.zeros((2)) for i in range(0, PAST_HEADINGS_N): s += PAST_HEADINGS[i] o.heading = (math.atan2(s[0], s[1]) * 360/(2*math.pi) + 360) % 360 PAST_HEADINGS_I = (PAST_HEADINGS_I + 1) % PAST_HEADINGS_N o.uuid = random.randint(-sys.maxsize - 1, sys.maxsize) print('send heading: {0}'.format(o.heading)) shm.hydrophones_filtered.set(o) random.seed() watch_thread_wrapper(loop)
print("{}, but another type was assumed. This is most likely a bug with auv-shn-cli.".format(e)) print(" {}.{} is currently {}".format(args.group, args.variable, variable.get())) if args.watch: def f(watcher, quit_event): watcher.watch(group) string = " {}.{} is currently {{}}".format(args.group, args.variable) while True: watcher.wait(new_update=False) if quit_event.is_set(): break print(string.format(variable.get())) watch_thread_wrapper(f) # No variable given, display all variables in the group else: if args.reset: with open(os.path.join(os.getenv('CUAUV_SOFTWARE'), 'libshm/c/shm.c')) as f: for line in f: if "shm_set({}".format(args.group) in line: variable = line.split("shm_set({}, ".format(args.group))[1].split(")")[0].split(", ") call(["auv-shm-cli", args.group] + variable) else: print_group(args.group, group) if args.watch: def f(watcher, quit_event): watcher.watch(group)
def main(args): if args['rate']: step_time = 1.0 / args['rate'] log("Starting controller at %f HZ, one step every %f seconds" % (args['rate'], step_time), copy_to_stdout=True) else: log("Starting controller with kalman lock", copy_to_stdout=True) load_settings() 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() watch_thread_wrapper(loop)
def main(args): if args['rate']: step_time = 1.0 / args['rate'] print("Starting controller at %f HZ, one step every %f seconds" % (args['rate'], step_time)) else: print("Starting controller with kalman lock") 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() tm.update(g) # Get passive forces and passive torques on the sub in world space passives = 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: 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() dt_pid = pid.step() # execute one PID step for all PID controllers # Get the correct inertia tensor of the sub in world coordinates I = vehicle.get_inertia_tensor(tm.orientation) # Controller step dt_opt = opt.set_motors(tm, passives, I) total_time = time.time() - start_time if args['verbose']: sys.stdout.write("PID step ran in %0.2f HZ\n" % (1 / dt_pid)) sys.stdout.write("Optimizer ran in %0.2f HZ\n" % (1 / dt_opt)) sys.stdout.write("Total rate: %0.2f HZ\n" % (1 / total_time)) sys.stdout.flush() if args['rate']: total_time = dt_pid + dt_opt if total_time > step_time: print(str(datetime.now()) + \ "## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time)) else: if args['verbose']: print("Rate capped at %f HZ" % args['rate']) print("-----") #visual break time.sleep(step_time - total_time) pid.clean() zero_motors() watch_thread_wrapper(loop)
localizer = Localizer( shm.hydrophones_settings.track_frequency_target.get()) while not quit_event.is_set(): # input() results = shm.hydrophones_results_track.get() kalman = shm.kalman.get() sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) print("------------------------------------------------------") localizer.add_observation( (results.diff_phase_x, results.diff_phase_y), sub_pos, sub_quat) pinger_pos, pinger_pos_all = localizer.compute_position() print("%d observations. current position: %0.2f %0.2f %0.2f" % \ (len(localizer.observations), sub_pos[0], sub_pos[1], sub_pos[2])) print("Predicted (2 Trans): %0.2f %0.2f %0.2f" % \ (pinger_pos[0], pinger_pos[1], pinger_pos[2])) print("Predicted (3 Trans): %0.2f %0.2f %0.2f" % \ (pinger_pos_all[0], pinger_pos_all[1], pinger_pos_all[2])) watcher.wait(new_update=False) shm.hydrophones_settings.track_frequency_target.set(PINGER_FREQ) shm.hydrophones_settings.track_magnitude_threshold.set(TRACK_MAG_THRESH) watch_thread_wrapper(func)
#!/usr/bin/env python2 import shm from misc.utils import watch_thread_wrapper ''' Displays mission messages from shared memory (useful for playback) Jeff Heidel 2012 ''' def missionPrinter(watcher, quit_event): watcher.watch(shm.mission) current = "" while not quit_event.is_set(): new = shm.mission.task_messages.get() if new != current: current = new print current watcher.wait() watch_thread_wrapper(missionPrinter)
#!/usr/bin/env python2 """ Starts a new shmlog every time the vehicle enters the water. Alex Spitzer 2014 """ import subprocess import shm from misc.utils import watch_thread_wrapper LOG_CMD = "auv-shmlogd" def uptime_watcher(w, quit_event): w.watch(shm.uptime) log_process = None while not quit_event.is_set(): in_water = shm.uptime.in_water.get() if in_water and log_process is None: log_process = subprocess.Popen(LOG_CMD) elif not in_water and log_process is not None: log_process.terminate() log_process = None w.wait(False) watch_thread_wrapper(uptime_watcher)
print "DEBUG: Shapes Matched: \n\t%s" % ("\n\t".join(["%d: %s (%.3f)" % (i+1,x,p) for i,(x,p) in enumerate(zip(shapes,shape_out_probs))])) return [(name, (x, y, prob)) for name, (x,y), prob in zip(shapes, positions, shape_out_probs)] if __name__ == "__main__": def daemon(w, quit_event): print "== Bins Machine Learning Classifier Daemon ==" print "Loading SVM data..." c = Classifier() print "SVM data loaded!" w.watch(shm.shape_results_4) w.watch(shm.shape_settings) while 1: w.wait(True) if quit_event.is_set(): break bins_set = [] if shm.shape_settings.enabled: for (name, (x, y, prob)) in c.classify(): bins_set.append(name) set_shm_for_key(name, x, y, prob) for name in set(bin_options.keys()) - set(bins_set): set_shm_for_key(name, 0, 0, 0.0) watch_thread_wrapper(daemon)