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)
