def main(args):
com = Communicator(module_name=args.module_name)
#incoming_packet = {"forward/backward": 0.0, "right/left": 0.0, "up/down": 0.0, "yaw": 0.0, "roll": 0.0, "pitch": 0.0}
last_packet_time = 0
while True:
incoming_packet = com.get_last_message("movement/fuzzification")
if incoming_packet and incoming_packet['timestamp'] > last_packet_time:
last_packet_time = incoming_packet['timestamp']
try:
outgoing_packet = {"front_left": 0.0, "front_right": 0.0, "middle_left": 0.0, "middle_right": 0.0, "back_left": 0.0, "back_right": 0.0}
# Possible TODO: Scale all values except for forward/backward by some constant so yaw, strafe, etc, are easier to control
outgoing_packet["front_left"] = incoming_packet["Fuzzy_Sets"]["forward/backward"] + incoming_packet["Fuzzy_Sets"]["right/left"] + incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["front_right"] = incoming_packet["Fuzzy_Sets"]["forward/backward"] - incoming_packet["Fuzzy_Sets"]["right/left"] - incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["back_left"] = incoming_packet["Fuzzy_Sets"]["forward/backward"] - incoming_packet["Fuzzy_Sets"]["right/left"] + incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["back_right"] = incoming_packet["Fuzzy_Sets"]["forward/backward"] + incoming_packet["Fuzzy_Sets"]["right/left"] - incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["middle_left"] = incoming_packet["Fuzzy_Sets"]["up/down"] + incoming_packet["Fuzzy_Sets"]["roll"]
outgoing_packet["middle_right"] = incoming_packet["Fuzzy_Sets"]["up/down"] - incoming_packet["Fuzzy_Sets"]["roll"]
for key in outgoing_packet.keys():
if outgoing_packet[key] > 1.0: outgoing_packet[key] = 1.0
if outgoing_packet[key] < -1.0: outgoing_packet[key] = -1.0
print outgoing_packet
com.publish_message({"Defuzzified_Sets": outgoing_packet})
except KeyError as e:
pass
time.sleep(args.epoch)
def __init__(self, module_name, settings, pipe):
"""Initialize video capture, logger, and processing plugins.
Args:
module_name -- TODO
settings -- dict of settings for this object.
pipe -- TODO
"""
self.module_name = module_name
self.master_settings = settings['sensor/vision/control']
self.settings = settings[module_name]
self.processor = FrameProcessor()
self._com = Communicator(module_name)
# Nagging messages from the parent come from here. The process must
# respond quickly enough or be killed by the parent.
self._pipe = pipe
self._fresh_frame_available = Event()
self._got_im, self._im = None, None
self._init_stream()
# Individual stream images will be processed by modules stored here.
self._plugins = []
# Load, instantiate, and store the modules defined in the settings file.
for plugin_name in self.settings['plugins']:
# Name of module and the class should be the same.
module_obj = getattr(
import_module('..' + plugin_name, package='plugins.subpkg'),
plugin_name)(self.processor, self.settings)
self._plugins += [module_obj]
# Configure the VideoCapture object.
self._vision_process()
def main(args):
com = Communicator(module_name=args.module_name)
fuzzy_sets = settings[args.module_name]["fuzzy_sets"]
# These values represent the submarine's membership in 8 fuzzy sets.
# These sets come in pairs (left/right, back/forward, etc.) and represent
# where the submarine is in relation to where it wants to be.
# The roll and yaw sets are not available to directive module since
# they should be controlled by either the stabilization module, or by
# the awesome balancing skills of the mech-e's.
# Expected packet sent by this module:
# packet = {
# 'is_left': 0.0,
# 'is_right': 0.0,
# 'is_back': 0.0,
# 'is_forward': 0.0,
# 'is_low': 0.0,
# 'is_high': 0.0,
# 'is_rotated_left': 0.0,
# 'is_rotated_right': 0.0}
# Received missive is of the form:
# {"desired_offset": {"x": 0.0, "y": 0.0, "z": 0.0},
# "desired_orientation": {"yaw": 0.0, "pitch": 0.0, "roll": 0.0},
# "desired_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
# "face_of_power": self.face_of_power,
# "timestamp": time.time()}
last_timestamp = 0.0
while True:
# TODO: Figure out AI module name
missive = com.get_last_message("decision")
if missive and missive['timestamp'] > last_timestamp:
last_timestamp = missive['timestamp']
packet = {}
packet['is_left'] = get_membership(missive['desired_offset']['x'],
fuzzy_sets['is_left'])
packet['is_right'] = get_membership(missive['desired_offset']['x'],
fuzzy_sets['is_right'])
packet['is_back'] = get_membership(missive['desired_offset']['y'],
fuzzy_sets['is_back'])
packet['is_forward'] = get_membership(
missive['desired_offset']['y'], fuzzy_sets['is_forward'])
packet['is_low'] = get_membership(missive['desired_offset']['z'],
fuzzy_sets['is_low'])
packet['is_high'] = get_membership(missive['desired_offset']['z'],
fuzzy_sets['is_high'])
packet['is_rotated_left'] = get_membership(
missive['desired_orientation']['yaw'],
fuzzy_sets['is_rotated_left'])
packet['is_rotated_right'] = get_membership(
missive['desired_orientation']['yaw'],
fuzzy_sets['is_rotated_right'])
com.publish_message(packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(module_name=args.module_name)
fuzzy_sets = settings[args.module_name]["fuzzy_sets"]
# These values represent the submarine's membership in 8 fuzzy sets.
# These sets come in pairs (left/right, back/forward, etc.) and represent
# where the submarine is in relation to where it wants to be.
# The roll and yaw sets are not available to directive module since
# they should be controlled by either the stabilization module, or by
# the awesome balancing skills of the mech-e's.
# Expected packet sent by this module:
# packet = {
# 'is_left': 0.0,
# 'is_right': 0.0,
# 'is_back': 0.0,
# 'is_forward': 0.0,
# 'is_low': 0.0,
# 'is_high': 0.0,
# 'is_rotated_left': 0.0,
# 'is_rotated_right': 0.0}
# Received missive is of the form:
# {"desired_offset": {"x": 0.0, "y": 0.0, "z": 0.0},
# "desired_orientation": {"yaw": 0.0, "pitch": 0.0, "roll": 0.0},
# "desired_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
# "face_of_power": self.face_of_power,
# "timestamp": time.time()}
last_timestamp = 0.0
while True:
# TODO: Figure out AI module name
missive = com.get_last_message("decision")
if missive and missive['timestamp'] > last_timestamp:
last_timestamp = missive['timestamp']
packet = {}
packet['is_left'] = get_membership(
missive['desired_offset']['x'], fuzzy_sets['is_left'])
packet['is_right'] = get_membership(
missive['desired_offset']['x'], fuzzy_sets['is_right'])
packet['is_back'] = get_membership(
missive['desired_offset']['y'], fuzzy_sets['is_back'])
packet['is_forward'] = get_membership(
missive['desired_offset']['y'], fuzzy_sets['is_forward'])
packet['is_low'] = get_membership(
missive['desired_offset']['z'], fuzzy_sets['is_low'])
packet['is_high'] = get_membership(
missive['desired_offset']['z'], fuzzy_sets['is_high'])
packet['is_rotated_left'] = get_membership(
missive['desired_orientation']['yaw'],
fuzzy_sets['is_rotated_left'])
packet['is_rotated_right'] = get_membership(
missive['desired_orientation']['yaw'],
fuzzy_sets['is_rotated_right'])
com.publish_message(packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(args.module_name)
controller = Controller(
["X1", "Y1", "X2", "Y2", "R2", "L2"],
["right/left", "forward/backward", "yaw", "pitch", "up", "down"],
(0, 255), (-1, 1))
while True:
control_packet = controller.get_values()
try:
outgoing_packet = {
"right/left": 0.0,
"forward/backward": 0.0,
"yaw": 0.0,
"pitch": 0.0,
"up/down": 0.0,
"roll": 0.0
}
# Further parse controller values here
# Controller's sticks Y axis are switched
control_packet[
"forward/backward"] = -control_packet["forward/backward"]
control_packet["pitch"] = -control_packet["pitch"]
# Up and Down are not -1 to 1. Just 0 - 1
control_packet["up"] = controller.map_range(
control_packet["up"], -1, 1, 0, 1)
control_packet["down"] = controller.map_range(
control_packet["down"], -1, 1, 0, -1)
# Transferring to outgoing packet
outgoing_packet["forward/backward"] = control_packet[
"forward/backward"]
outgoing_packet["right/left"] = control_packet["right/left"]
outgoing_packet[
"up/down"] = control_packet["up"] + control_packet["down"]
outgoing_packet["yaw"] = control_packet["yaw"]
outgoing_packet["pitch"] = control_packet["pitch"]
#outgoing_packet["roll"] = control_packet["roll"]
outgoing_packet["roll"] = 0.0
# Controller sticks are not centered very well.
# TODO: Find a better way to do this (short of getting a new controller)
for key in outgoing_packet.keys():
if abs(outgoing_packet[key]) < .10: outgoing_packet[key] = 0.0
print outgoing_packet
Fuzzy_Sets = {"Fuzzy_Sets": outgoing_packet}
com.publish_message(Fuzzy_Sets)
except KeyError as i:
pass
sleep(args.epoch)
def main(args):
com = Communicator(module_name=args.module_name,
settings_path=args.settings_path)
# TODO We need to settle on a good convention for these vectors.
# For now, I'm using these conventions:
# vector:
# x is left and right
# y is forward and backward
# z is up and down.
# rotation:
# x is pitch
# y is roll
# z is yaw. Negative yaws left, positive yaws right.
packet = {
'vector': {
'x': 0.0,
'y': 0.0,
'z': 0.0
},
'rotation': {
'x': 0.0,
'y': 0.0,
'z': 0.0
}
}
last_packet_time = 0.0
while True:
directive_packet = com.get_last_message("movement/directive")
if directive_packet and directive_packet[
'timestamp'] > last_packet_time:
last_packet_time = directive_packet['timestamp']
# Kludges to handle keyboard inputs.
tx_packet = deepcopy(packet)
if directive_packet['is_left'] == 1.0:
tx_packet['vector']['x'] = 1.0
elif directive_packet['is_right'] == 1.0:
tx_packet['vector']['x'] = -1.0
elif directive_packet['is_back'] == 1.0:
tx_packet['vector']['y'] = 1.0
elif directive_packet['is_forward'] == 1.0:
tx_packet['vector']['y'] = -1.0
elif directive_packet['is_low'] == 1.0:
tx_packet['vector']['z'] = 1.0
elif directive_packet['is_high'] == 1.0:
tx_packet['vector']['z'] = -1.0
elif directive_packet['is_rotated_left'] == 1.0:
tx_packet['rotation']['z'] = 1.0
elif directive_packet['is_rotated_right'] == 1.0:
tx_packet['rotation']['z'] = -1.0
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(
module_name=args.module_name,
settings_path=args.settings_path)
# These values represent the submarine's membership in 8 fuzzy sets.
# These sets come in pairs (left/right, back/forward, etc.) and represent
# where the submarine is in relation to where it wants to be.
# The roll and yaw sets are not available to directive module since
# they should be controlled by either the stabilization module, or by
# the awesome balancing skills of the mech-e's.
packet = {
'is_left': 0.0,
'is_right': 0.0,
'is_back': 0.0,
'is_forward': 0.0,
'is_low': 0.0,
'is_high': 0.0,
'is_rotated_left': 0.0,
'is_rotated_right': 0.0}
missive = {
"desired_offset": {"x": 0.0, "y": 0.0, "z": 0.0},
"desired_orientation": {"x": 0.0, "y": 0.0, "z": 0.0},
"desired_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
"timestamp": time.time()}
last_timestamp = 0.0
while True:
missive = com.get_last_message("decision")
if missive and missive['timestamp'] > last_timestamp:
last_timestamp = missive['timestamp']
tx_packet = deepcopy(packet)
if (missive['desired_offset']['y'] == 9001.0 and
missive['desired_velocity']['y'] == 1.0):
tx_packet['is_back'] = 1.0
elif (missive['desired_offset']['y'] == -9001 and
missive['desired_velocity']['y'] == -1.0):
tx_packet['is_forward'] = 1.0
elif missive['desired_orientation']['z'] == 3 * math.pi / 2:
tx_packet['is_rotated_right'] = 1.0
elif missive['desired_orientation']['z'] == math.pi / 2:
tx_packet['is_rotated_left'] = 1.0
elif (missive['desired_offset']['z'] == 9001 and
missive['desired_velocity']['z'] == 1.0):
tx_packet['is_low'] = 1.0
elif (missive['desired_offset']['z'] == -9001 and
missive['desired_velocity']['z'] == -1.0):
tx_packet['is_high'] = 1.0
print tx_packet
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(module_name=args.module_name)
#incoming_packet = {"forward/backward": 0.0, "right/left": 0.0, "up/down": 0.0, "yaw": 0.0, "roll": 0.0, "pitch": 0.0}
last_packet_time = 0
while True:
incoming_packet = com.get_last_message("movement/fuzzification")
if incoming_packet and incoming_packet['timestamp'] > last_packet_time:
last_packet_time = incoming_packet['timestamp']
try:
outgoing_packet = {
"front_left": 0.0,
"front_right": 0.0,
"middle_left": 0.0,
"middle_right": 0.0,
"back_left": 0.0,
"back_right": 0.0
}
# Possible TODO: Scale all values except for forward/backward by some constant so yaw, strafe, etc, are easier to control
outgoing_packet["front_left"] = incoming_packet["Fuzzy_Sets"][
"forward/backward"] + incoming_packet["Fuzzy_Sets"][
"right/left"] + incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["front_right"] = incoming_packet["Fuzzy_Sets"][
"forward/backward"] - incoming_packet["Fuzzy_Sets"][
"right/left"] - incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["back_left"] = incoming_packet["Fuzzy_Sets"][
"forward/backward"] - incoming_packet["Fuzzy_Sets"][
"right/left"] + incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["back_right"] = incoming_packet["Fuzzy_Sets"][
"forward/backward"] + incoming_packet["Fuzzy_Sets"][
"right/left"] - incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["middle_left"] = incoming_packet["Fuzzy_Sets"][
"up/down"] + incoming_packet["Fuzzy_Sets"]["roll"]
outgoing_packet[
"middle_right"] = incoming_packet["Fuzzy_Sets"][
"up/down"] - incoming_packet["Fuzzy_Sets"]["roll"]
for key in outgoing_packet.keys():
if outgoing_packet[key] > 1.0: outgoing_packet[key] = 1.0
if outgoing_packet[key] < -1.0: outgoing_packet[key] = -1.0
print outgoing_packet
com.publish_message({"Defuzzified_Sets": outgoing_packet})
except KeyError as e:
pass
time.sleep(args.epoch)
def main(args):
com = Communicator("movement/physical")
last_packet_time = 0.0
while True:
rx_packet = com.get_last_message("movement/stabilization")
if rx_packet and rx_packet['timestamp'] > last_packet_time:
last_packet_time = rx_packet['timestamp']
tx_packet = {
'vector': rx_packet['vector'],
'rotation': rx_packet['rotation']}
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
com = Communicator("movement/physical")
last_packet_time = 0.0
while True:
rx_packet = com.get_last_message("movement/stabilization")
if rx_packet and rx_packet['timestamp'] > last_packet_time:
last_packet_time = rx_packet['timestamp']
tx_packet = {
'vector': rx_packet['vector'],
'rotation': rx_packet['rotation']
}
com.publish_message(tx_packet)
time.sleep(args.epoch)
def __init__(self, module_name, settings, pipe):
"""Initialize video capture, logger, and processing plugins.
Args:
module_name -- TODO
settings -- dict of settings for this object.
pipe -- TODO
"""
self.module_name = module_name
self.master_settings = settings['sensor/vision/control']
self.settings = settings[module_name]
self.processor = FrameProcessor()
self._com = Communicator(module_name)
# Nagging messages from the parent come from here. The process must
# respond quickly enough or be killed by the parent.
self._pipe = pipe
self._fresh_frame_available = Event()
self._got_im, self._im = None, None
self._init_stream()
# Individual stream images will be processed by modules stored here.
self._plugins = []
# Load, instantiate, and store the modules defined in the settings file.
for plugin_name in self.settings['plugins']:
# Name of module and the class should be the same.
module_obj = getattr(import_module(
'..'+plugin_name, package='plugins.subpkg'),
plugin_name)(self.processor, self.settings)
self._plugins += [module_obj]
# Configure the VideoCapture object.
self._vision_process()
def main(args):
com = Communicator("movement/translation")
last_packet_time = 0.0
while True:
rx_packet = com.get_last_message("movement/defuzzification")
if rx_packet and rx_packet['timestamp'] > last_packet_time:
tx_packet = {'Thruster_Values': {}}
last_packet_time = rx_packet['timestamp']
for k in rx_packet['Defuzzified_Sets']:
tx_packet['Thruster_Values'][k] = \
translate(k, rx_packet['Defuzzified_Sets'][k])
com.publish_message(tx_packet)
print tx_packet
time.sleep(args.epoch)
def main(args):
com = Communicator("movement/translation")
last_packet_time = 0.0
while True:
rx_packet = com.get_last_message("movement/defuzzification")
if rx_packet and rx_packet['timestamp'] > last_packet_time:
tx_packet = {'Thruster_Values': {}}
last_packet_time = rx_packet['timestamp']
for k in rx_packet['Defuzzified_Sets']:
tx_packet['Thruster_Values'][k] = \
translate(k, rx_packet['Defuzzified_Sets'][k])
com.publish_message(tx_packet)
print tx_packet
time.sleep(args.epoch)
def __init__(self, calibration_samples, com_module_name, epoch):
threading.Thread.__init__(self)
self.daemon = True
self.calibration_samples = calibration_samples
self.com = Communicator(module_name=com_module_name)
self.calibrated = False
self.epoch = epoch
def __init__(self):
self.com = Communicator(module_name="util/vision_viewer",
subscriber_buffer_length=81920,
subscriber_high_water_mark=81920,
settings_path="../../settings.json")
last_timestamp = 0.0
self.com.connect_video_stream(50000)
print 'connected'
result = []
while True:
try:
im = self.com.recv_image()
except ValueError:
im = None
if im is not None:
print 'got an image'
cv2.imshow('test window', im)
key = cv2.waitKey(1)
def main(args):
com = Communicator(module_name=args.module_name)
# TODO We need to settle on a good convention for these vectors.
# For now, I'm using these conventions:
# vector:
# x is left and right
# y is forward and backward
# z is up and down.
# rotation:
# x is pitch
# y is roll
# z is yaw. Negative yaws left, positive yaws right.
packet = {"vector": {"x": 0.0, "y": 0.0, "z": 0.0}, "rotation": {"yaw": 0.0, "pitch": 0.0, "roll": 0.0}}
last_packet_time = 0.0
while True:
directive_packet = com.get_last_message("movement/directive")
if directive_packet and directive_packet["timestamp"] > last_packet_time:
last_packet_time = directive_packet["timestamp"]
# Kludges to handle keyboard inputs.
tx_packet = deepcopy(packet)
if directive_packet["is_left"] > 0.0:
tx_packet["vector"]["x"] = 1.0
elif directive_packet["is_right"] > 0.0:
tx_packet["vector"]["x"] = -1.0
elif directive_packet["is_back"] > 0.0:
tx_packet["vector"]["y"] = 1.0
elif directive_packet["is_forward"] > 0.0:
tx_packet["vector"]["y"] = -1.0
elif directive_packet["is_low"] > 0.0:
tx_packet["vector"]["z"] = 1.0
elif directive_packet["is_high"] > 0.0:
tx_packet["vector"]["z"] = -1.0
elif directive_packet["is_rotated_left"] > 0.0:
tx_packet["rotation"]["yaw"] = 1.0
elif directive_packet["is_rotated_right"] > 0.0:
tx_packet["rotation"]["yaw"] = -1.0
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(module_name="util/logger")
com.publish_message("logger.py started")
try:
check_call(['mv', args.output,
"/home/robosub/logging/log.out." + str(time.time())])
except CalledProcessError:
pass
with open(args.output, 'w') as log_file:
while True:
for mname in com.listening():
for message in com.get_messages(mname):
json.dump(message, log_file)
log_file.write('\n')
log_file.flush()
sleep(args.epoch)
log_file.close()
class VisionViewer(object):
def __init__(self):
self.com = Communicator(
module_name="util/vision_viewer",
subscriber_buffer_length=81920,
subscriber_high_water_mark=81920,
settings_path="../../settings.json")
last_timestamp = 0.0
self.com.connect_video_stream(50000)
print 'connected'
result = []
while True:
try:
im = self.com.recv_image()
except ValueError:
im = None
if im is not None:
print 'got an image'
cv2.imshow('test window', im)
key = cv2.waitKey(1)
def main (args):
com = Communicator (args.module_name)
controller = Controller (["X1", "Y1", "X2", "Y2", "R2", "L2"], ["right/left", "forward/backward", "yaw", "pitch", "up", "down"], (0, 255), (-1, 1))
while True:
control_packet = controller.get_values ()
try:
outgoing_packet = {"right/left": 0.0, "forward/backward": 0.0, "yaw": 0.0, "pitch": 0.0, "up/down": 0.0, "roll": 0.0}
# Further parse controller values here
# Controller's sticks Y axis are switched
control_packet["forward/backward"] = -control_packet["forward/backward"]
control_packet["pitch"] = -control_packet["pitch"]
# Up and Down are not -1 to 1. Just 0 - 1
control_packet["up"] = controller.map_range(control_packet["up"], -1, 1, 0, 1)
control_packet["down"] = controller.map_range(control_packet["down"], -1, 1, 0, -1)
# Transferring to outgoing packet
outgoing_packet["forward/backward"] = control_packet["forward/backward"]
outgoing_packet["right/left"] = control_packet["right/left"]
outgoing_packet["up/down"] = control_packet["up"] + control_packet["down"]
outgoing_packet["yaw"] = control_packet["yaw"]
outgoing_packet["pitch"] = control_packet["pitch"]
#outgoing_packet["roll"] = control_packet["roll"]
outgoing_packet["roll"] = 0.0
# Controller sticks are not centered very well.
# TODO: Find a better way to do this (short of getting a new controller)
for key in outgoing_packet.keys ():
if abs (outgoing_packet[key]) < .10: outgoing_packet[key] = 0.0
print outgoing_packet
Fuzzy_Sets = {"Fuzzy_Sets": outgoing_packet}
com.publish_message (Fuzzy_Sets)
except KeyError as i:
pass
sleep (args.epoch)
class VisionViewer(object):
def __init__(self):
self.com = Communicator(module_name="util/vision_viewer")
# subscriber_buffer_length=81920,
# subscriber_high_water_mark=81920)
last_timestamp = 0.0
self.com.connect_video_stream(50001, "192.168.1.3")
# self.com.connect_video_stream(50001)
print "connected"
while True:
print "hmm"
try:
im = self.com.recv_image()
except ValueError:
sleep(0.1)
im = None
if im is not None:
print "got an image"
b, g, r = cv2.split(im)
cv2.imshow("test window", cv2.subtract(r, b))
key = cv2.waitKey(1)
def __init__(self):
self.com = Communicator(
module_name="util/vision_viewer")
# subscriber_buffer_length=81920,
# subscriber_high_water_mark=81920)
last_timestamp = 0.0
self.com.connect_video_stream(50001, '192.168.1.3')
#self.com.connect_video_stream(50001)
print 'connected'
while True:
print 'hmm'
try:
im = self.com.recv_image()
except ValueError:
sleep(.1)
im = None
if im is not None:
print 'got an image'
b, g, r = cv2.split(im)
cv2.imshow('test window', cv2.subtract(r, b))
key = cv2.waitKey(1)
class Sensor(threading.Thread):
__metaclass__ = ABCMeta
def __init__(self, calibration_samples, com_module_name, epoch):
threading.Thread.__init__(self)
self.daemon = True
self.calibration_samples = calibration_samples
self.com = Communicator(module_name=com_module_name)
self.calibrated = False
self.epoch = epoch
def run(self):
while not self.calibrated:
self.com.debug("Calibrating {0}...".format(type(self)))
self.calibrate(self.calibration_samples)
self.sleep()
while True:
self.com.publish_message(self.fetch_data())
self.com.debug('sleeping')
self.sleep()
@abstractmethod
def fetch_data(self):
pass
@abstractmethod
def calibrate(self, samples=10):
pass
def sleep(self):
time.sleep(self.epoch)
def main(args):
com = Communicator(module_name="util/logger")
com.publish_message("logger.py started")
try:
check_call([
'mv', args.output,
"/home/robosub/logging/log.out." + str(time.time())
])
except CalledProcessError:
pass
with open(args.output, 'w') as log_file:
while True:
for mname in com.listening():
for message in com.get_messages(mname):
json.dump(message, log_file)
log_file.write('\n')
log_file.flush()
sleep(args.epoch)
log_file.close()
def main(args):
"""The resultant command is communicated over the grapevine in a
dicitonary with this format:
{"desired_offset":
{"x": 0.0,
"y": 0.0,
"z": 0.0},
"desired_orientation":
{"yaw": 0.0,
"pitch": 0.0,
"roll": 0.0},
"desired_velocity":
{"x": 0.0,
"y": 0.0,
"z": 0.0}
}
yaw, pitch, and roll are in radians. A small positive yaw specifies
a desire to turn right, a small possitive pitch specifies a desire
to tilt the nose up, and a small positive roll specifies a desire
to lower the right side of the sub.
The AI's job is to interpret sensor data and vision information,
and from that, specify where the sub SHOULD be relative to where
it CURRENTLY is. The desired location represents a state that
should be achieved at some future time.
The vector "desired_offset" specifies the relative location of
that desired state, the "desired_orientation" specifies which
direction the submarine should be facing, and the "desired_velocity"
specifies how the submarine should be moving at that point.
This information structure intentionally doesn't allow the AI to specify
the "roll" axis or a desired rotational velocity.
One way to think about this is that the AI module needs to figure out
what the next waypoint is, and where that waypoint is relative to
where the submarine currenlty is stationed.
This module should be something of a state machine. Given a specific
state, one of the Oligarch classes should be able to decide what to
do, but something needs to decide which oligarch is in charge.
"""
com = Communicator(module_name=args.module_name)
oligarchs = {
"AdvisorsPeon": AdvisorsPeon(com),
"PathOligarch": PathOligarch(com),
"DepthOligarch": DepthOligarch(com),
}
state = None
vision_cam_front = None
vision_cam_down = None
advice = None
depth = None
while True:
advice, success = choose_last_packet(com, "decision/advisor", advice)
if success:
if advice["command"] == "state: keyboard":
state = 'keyboard'
elif advice["command"] == "state: path":
state = 'path'
elif advice["command"] == "state: depth":
state = 'depth'
elif advice["command"] == "stop":
state = "stop"
vision_cam_front, _ = choose_last_packet(com,
"sensor/vision/cam_front",
vision_cam_front)
vision_cam_down, _ = choose_last_packet(com, "sensor/vision/cam_down",
vision_cam_front)
# TODO: Allow this to look at sanitized data.
depth, _ = choose_last_packet(com, "datafeed/raw/depth", depth)
if state == "stop":
pass
elif state == 'keyboard' and success:
oligarchs["AdvisorsPeon"].decision(advice)
elif state == 'path':
oligarchs["PathOligarch"].decision(vision_cam_front,
vision_cam_down)
elif state == 'depth':
oligarchs["DepthOligarch"].decision(depth, advice)
time.sleep(args.epoch)
def main(args):
print """Usage:
+-------+-------+-------+-------+
| 1 | 2 | 3 | 4 | magnitude will always be between
| | | | | 0 and 127. Use '[' to decrease the
|KEYBOARD PATH | DEPTH | N/A | magnitude and ']' to increase it.
++------++------++------++------++
| q | w | e | r | Use '-' to decrease desired depth.
| | | | | Use '=' to increase desired depth.
| QUIT |FORWARD| STOP | RISE |
++------++------++------++------++
| a | s | d | f |
| ROTATE| | ROTATE| |
| LEFT | BACK | RIGHT | FALL |
+-------+-------+-------+-------+
"""
com = Communicator(module_name=args.module_name)
advice_template = {"command": None}
magnitude = 91
desired_depth = 500
while True:
key = getch()
msg = "magnitude: {0:3} last key: {1} desired depth: {2}\r"
msg = msg.format(magnitude, key, desired_depth)
print msg,
advice = deepcopy(advice_template)
if key == 'w':
advice["command"] = "forward"
elif key == 'a':
advice["command"] = "yaw left"
elif key == 's':
advice["command"] = "backward"
elif key == 'd':
advice["command"] = "yaw right"
elif key == 'r':
advice["command"] = "rise"
elif key == 'f':
advice["command"] = "descend"
elif key == 't':
advice["command"] = "roll left"
elif key == 'g':
advice["command"] = "roll right"
elif key == 'e':
advice["command"] = "stop"
elif key == '1':
advice["command"] = "state: keyboard"
elif key == '2':
advice["command"] = "state: path"
elif key == '3':
advice["command"] = "state: depth"
elif key == '[':
magnitude -= 1
elif key == ']':
magnitude += 1
elif key == '-':
desired_depth -= 1
elif key == '=':
desired_depth += 1
#elif key == '6':
# advice["command"] =
#elif key == '7':
# advice["command"] =
#elif key == '8':
# advice["command"] =
#elif key == '9':
# advice["command"] =
elif key == 'q':
sys.exit()
if magnitude > 127:
magnitude = 127
if magnitude < 0:
magnitude = 0
advice["magnitude"] = magnitude
advice["desired_depth"] = desired_depth
com.publish_message(advice)
def main(args):
print """Usage:
+-------+-------+-------+-------+
| 1 | 2 | 3 | 4 | magnitude will always be between
| | | | | 0 and 127. Use '[' to decrease the
|KEYBOARD PATH | DEPTH | N/A | magnitude and ']' to increase it.
++------++------++------++------++
| q | w | e | r | Use '-' to decrease desired depth.
| | | | | Use '=' to increase desired depth.
| QUIT |FORWARD| STOP | RISE |
++------++------++------++------++
| a | s | d | f |
| ROTATE| | ROTATE| |
| LEFT | BACK | RIGHT | FALL |
+-------+-------+-------+-------+
"""
com = Communicator(module_name=args.module_name)
advice_template = {"command": None}
magnitude = 91
desired_depth = 500
while True:
key = getch()
msg = "magnitude: {0:3} last key: {1} desired depth: {2}\r"
msg = msg.format(magnitude, key, desired_depth)
print msg,
advice = deepcopy(advice_template)
if key == "w":
advice["command"] = "forward"
elif key == "a":
advice["command"] = "yaw left"
elif key == "s":
advice["command"] = "backward"
elif key == "d":
advice["command"] = "yaw right"
elif key == "r":
advice["command"] = "rise"
elif key == "f":
advice["command"] = "descend"
elif key == "t":
advice["command"] = "roll left"
elif key == "g":
advice["command"] = "roll right"
elif key == "e":
advice["command"] = "stop"
elif key == "1":
advice["command"] = "state: keyboard"
elif key == "2":
advice["command"] = "state: path"
elif key == "3":
advice["command"] = "state: depth"
elif key == "[":
magnitude -= 1
elif key == "]":
magnitude += 1
elif key == "-":
desired_depth -= 1
elif key == "=":
desired_depth += 1
# elif key == '6':
# advice["command"] =
# elif key == '7':
# advice["command"] =
# elif key == '8':
# advice["command"] =
# elif key == '9':
# advice["command"] =
elif key == "q":
sys.exit()
if magnitude > 127:
magnitude = 127
if magnitude < 0:
magnitude = 0
advice["magnitude"] = magnitude
advice["desired_depth"] = desired_depth
com.publish_message(advice)
class StreamProcessor(object):
"""Open a video stream and process frames. Report finds to the grapevine."""
def __init__(self, module_name, settings, pipe):
"""Initialize video capture, logger, and processing plugins.
Args:
module_name -- TODO
settings -- dict of settings for this object.
pipe -- TODO
"""
self.module_name = module_name
self.master_settings = settings['sensor/vision/control']
self.settings = settings[module_name]
self.processor = FrameProcessor()
self._com = Communicator(module_name)
# Nagging messages from the parent come from here. The process must
# respond quickly enough or be killed by the parent.
self._pipe = pipe
self._fresh_frame_available = Event()
self._got_im, self._im = None, None
self._init_stream()
# Individual stream images will be processed by modules stored here.
self._plugins = []
# Load, instantiate, and store the modules defined in the settings file.
for plugin_name in self.settings['plugins']:
# Name of module and the class should be the same.
module_obj = getattr(import_module(
'..'+plugin_name, package='plugins.subpkg'),
plugin_name)(self.processor, self.settings)
self._plugins += [module_obj]
# Configure the VideoCapture object.
self._vision_process()
def _init_stream(self):
"""Initialize the video stream."""
if self.settings['stream_type'] == 'device':
cam_path = os.path.realpath(self.settings['symlink'])
# Check that the symlink points to something that really exists.
if '/dev/video' not in cam_path:
raise Exception('Broken symlink {sym}'.format(
sym=self.settings['symlink']))
self._cap = cv2.VideoCapture(int(cam_path[-1]))
self._cap.set(cv.CV_CAP_PROP_FPS, self.settings['fps'])
self._frame_grabber_thread = Thread(target=self.refresh_frame)
self._frame_grabber_thread.daemon = True
self._frame_grabber_thread.start()
#self._frame_grabber_thread = None
elif self.settings['stream_type'] == 'recorded_video':
# The 'symlink' key didn't exist. This must be a test with recorded
# video.
self._cap = cv2.VideoCapture(self.settings['recorded_video'])
self._frame_grabber_thread = None
else:
raise Exception(
'No stream type specified for {module_name}'.format(
module_name=module_name))
def _get_frame(self):
"""Get the most recent frame from the video stream."""
if self._frame_grabber_thread is not None:
self._fresh_frame_available.wait()
return (self._got_im, self._im)
else:
return self._cap.read()
def refresh_frame(self):
"""Get and store latest frame from video stream as fast as possible."""
failed_count = 0
# Assuming 30fps, 150 fails in a row means 5s of no video.
while failed_count < self.master_settings['max_failed_frames']:
self._got_im, self._im = self._cap.read()
self._fresh_frame_available.set()
self._fresh_frame_available.clear()
if not self._got_im:
failed_count += 1
else:
failed_count = 0
raise Exception('Camera sucks')
def _vision_process(self):
"""Detect obstacle details from video stream. Report findings."""
while True:
start_time = time()
got_im, im = self._get_frame()
#print 'new frame'
if self._pipe.poll() is True:
# Send the message back to parent to prove this process
# isn't frozen.
self._pipe.send(self._pipe.recv())
if got_im:
# Check if parent sent a message over the pipe.
self.processor.load_im(im)
# self.processor.hacky_display() # FIXME this needs to go away.
packet = {}
for plugin in self._plugins:
plugin.process_image(packet)
self._com.publish_message(packet)
def main(args):
# Someone SHOULD complain about this.
global DEBUG
DEBUG = args.debug
com = Communicator("microcontroller")
accel_com = Communicator('datafeed/raw/accelerometer')
gyro_com = Communicator('datafeed/raw/gyroscope')
compass_com = Communicator('datafeed/raw/compass')
depth_com = Communicator('datafeed/raw/depth')
battery_voltage_com = Communicator('datafeed/raw/battery_voltage')
# If we want to insert a mock module for these sensors, we want to prevent
# multiple input sources from inserting messages into the pipe.
disabled_publish = lambda *args: None
if args.disable_accel_com:
accel_com.publish_message = disabled_publish
if args.disable_gyro_com:
gyro_com.publish_message = disabled_publish
if args.disable_compass_com:
compass_com.publish_message = disabled_publish
if args.disable_depth_com:
depth_com.publish_message = disabled_publish
if args.disable_battery_voltage_com:
battery_voltage_com.publish_message = disabled_publish
if not DEBUG:
ser = serial.Serial()
cmd_thruster.ser = ser
# this may change, depending on what port the OS gives the
# microcontroller
ser.port = args.port
# the baudrate may change in the future
ser.baudrate = args.baudrate
# attempt to open the serial port (there is no guard code, I'm assuming
# this does not fail)
ser.open()
get_lock(ser, com) # get in sync with the stream
prev_gv_timestamp = 0.0
while True:
gv_packet = com.get_last_message("movement/translation")
# handle values from grapevine.py
if gv_packet is not None and gv_packet['timestamp'] > prev_gv_timestamp:
cmd_thruster(THRUSTER_BOW_PORT, gv_packet["Thruster_Values"]["front_left"])
cmd_thruster(THRUSTER_BOW_SB, gv_packet["Thruster_Values"]["front_right"])
cmd_thruster(THRUSTER_DEPTH_PORT, gv_packet["Thruster_Values"]["middle_left"])
cmd_thruster(THRUSTER_DEPTH_SB, gv_packet["Thruster_Values"]["middle_right"])
cmd_thruster(THRUSTER_STERN_PORT, gv_packet["Thruster_Values"]["back_left"])
cmd_thruster(THRUSTER_STERN_SB, gv_packet["Thruster_Values"]["back_right"])
prev_gv_timestamp = gv_packet['timestamp']
# handle values from uC, USB port.
if not DEBUG:
uC_packet = get_packet(ser, com)
respond_to_serial_packet(uC_packet, accel_com, gyro_com, compass_com,
depth_com, battery_voltage_com)
time.sleep(args.epoch)
if not DEBUG:
ser.close()
#### Sanitation Module (sanitation.py)
#####
##### This module subscribes to all the sensors and uses the sensor data to create
##### a organized human-readable python dictionary for use throughout the system
#####
import json
import zmq
import os
import sys
import datetime
sys.path.append(os.path.abspath(".."))
from util.communication.grapevine import Communicator
san = Communicator(module_name="sensor/sanitation")
sensors = {}
# Example Sensor Object
#
# sensors = {
# 'gyroscope' : {
# 'gx' : 1,
# 'gy' : 2,
# 'gz' : 3
# },
# 'accelerometer' : {
# 'ax' : 1,
# 'ay' : 2,
# 'az' : 3
# },
# 'battery_voltage' : {
# 'voltage' : 5
def main(args):
# Someone SHOULD complain about this.
global DEBUG
DEBUG = args.debug
com = Communicator("microcontroller")
accel_com = Communicator('datafeed/raw/accelerometer')
gyro_com = Communicator('datafeed/raw/gyroscope')
compass_com = Communicator('datafeed/raw/compass')
depth_com = Communicator('datafeed/raw/depth')
battery_voltage_com = Communicator('datafeed/raw/battery_voltage')
# If we want to insert a mock module for these sensors, we want to prevent
# multiple input sources from inserting messages into the pipe.
disabled_publish = lambda *args: None
if args.disable_accel_com:
accel_com.publish_message = disabled_publish
if args.disable_gyro_com:
gyro_com.publish_message = disabled_publish
if args.disable_compass_com:
compass_com.publish_message = disabled_publish
if args.disable_depth_com:
depth_com.publish_message = disabled_publish
if args.disable_battery_voltage_com:
battery_voltage_com.publish_message = disabled_publish
if not DEBUG:
ser = serial.Serial()
cmd_thruster.ser = ser
# this may change, depending on what port the OS gives the
# microcontroller
ser.port = args.port
# the baudrate may change in the future
ser.baudrate = args.baudrate
# attempt to open the serial port (there is no guard code, I'm assuming
# this does not fail)
ser.open()
get_lock(ser, com) # get in sync with the stream
prev_gv_timestamp = 0.0
while True:
gv_packet = com.get_last_message("movement/translation")
# handle values from grapevine.py
if gv_packet is not None and gv_packet['timestamp'] > prev_gv_timestamp:
cmd_thruster(THRUSTER_BOW_PORT,
gv_packet["Thruster_Values"]["front_left"])
cmd_thruster(THRUSTER_BOW_SB,
gv_packet["Thruster_Values"]["front_right"])
cmd_thruster(THRUSTER_DEPTH_PORT,
gv_packet["Thruster_Values"]["middle_left"])
cmd_thruster(THRUSTER_DEPTH_SB,
gv_packet["Thruster_Values"]["middle_right"])
cmd_thruster(THRUSTER_STERN_PORT,
gv_packet["Thruster_Values"]["back_left"])
cmd_thruster(THRUSTER_STERN_SB,
gv_packet["Thruster_Values"]["back_right"])
prev_gv_timestamp = gv_packet['timestamp']
# handle values from uC, USB port.
if not DEBUG:
uC_packet = get_packet(ser, com)
respond_to_serial_packet(uC_packet, accel_com, gyro_com,
compass_com, depth_com,
battery_voltage_com)
time.sleep(args.epoch)
if not DEBUG:
ser.close()
def main(args):
com = Communicator(module_name=args.module_name,
settings_path=args.settings_path)
# These values represent the submarine's membership in 8 fuzzy sets.
# These sets come in pairs (left/right, back/forward, etc.) and represent
# where the submarine is in relation to where it wants to be.
# The roll and yaw sets are not available to directive module since
# they should be controlled by either the stabilization module, or by
# the awesome balancing skills of the mech-e's.
packet = {
'is_left': 0.0,
'is_right': 0.0,
'is_back': 0.0,
'is_forward': 0.0,
'is_low': 0.0,
'is_high': 0.0,
'is_rotated_left': 0.0,
'is_rotated_right': 0.0
}
missive = {
"desired_offset": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"desired_orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"desired_velocity": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"timestamp": time.time()
}
last_timestamp = 0.0
while True:
missive = com.get_last_message("decision")
if missive and missive['timestamp'] > last_timestamp:
last_timestamp = missive['timestamp']
tx_packet = deepcopy(packet)
if (missive['desired_offset']['y'] == 9001.0
and missive['desired_velocity']['y'] == 1.0):
tx_packet['is_back'] = 1.0
elif (missive['desired_offset']['y'] == -9001
and missive['desired_velocity']['y'] == -1.0):
tx_packet['is_forward'] = 1.0
elif missive['desired_orientation']['z'] == 3 * math.pi / 2:
tx_packet['is_rotated_right'] = 1.0
elif missive['desired_orientation']['z'] == math.pi / 2:
tx_packet['is_rotated_left'] = 1.0
elif (missive['desired_offset']['z'] == 9001
and missive['desired_velocity']['z'] == 1.0):
tx_packet['is_low'] = 1.0
elif (missive['desired_offset']['z'] == -9001
and missive['desired_velocity']['z'] == -1.0):
tx_packet['is_high'] = 1.0
print tx_packet
com.publish_message(tx_packet)
time.sleep(args.epoch)
class StreamProcessor(object):
"""Open a video stream and process frames. Report finds to the grapevine."""
def __init__(self, module_name, settings, pipe):
"""Initialize video capture, logger, and processing plugins.
Args:
module_name -- TODO
settings -- dict of settings for this object.
pipe -- TODO
"""
self.module_name = module_name
self.master_settings = settings['sensor/vision/control']
self.settings = settings[module_name]
self.processor = FrameProcessor()
self._com = Communicator(module_name)
# Nagging messages from the parent come from here. The process must
# respond quickly enough or be killed by the parent.
self._pipe = pipe
self._fresh_frame_available = Event()
self._got_im, self._im = None, None
self._init_stream()
# Individual stream images will be processed by modules stored here.
self._plugins = []
# Load, instantiate, and store the modules defined in the settings file.
for plugin_name in self.settings['plugins']:
# Name of module and the class should be the same.
module_obj = getattr(
import_module('..' + plugin_name, package='plugins.subpkg'),
plugin_name)(self.processor, self.settings)
self._plugins += [module_obj]
# Configure the VideoCapture object.
self._vision_process()
def _init_stream(self):
"""Initialize the video stream."""
if self.settings['stream_type'] == 'device':
cam_path = os.path.realpath(self.settings['symlink'])
# Check that the symlink points to something that really exists.
if '/dev/video' not in cam_path:
raise Exception('Broken symlink {sym}'.format(
sym=self.settings['symlink']))
self._cap = cv2.VideoCapture(int(cam_path[-1]))
self._cap.set(cv.CV_CAP_PROP_FPS, self.settings['fps'])
self._frame_grabber_thread = Thread(target=self.refresh_frame)
self._frame_grabber_thread.daemon = True
self._frame_grabber_thread.start()
#self._frame_grabber_thread = None
elif self.settings['stream_type'] == 'recorded_video':
# The 'symlink' key didn't exist. This must be a test with recorded
# video.
self._cap = cv2.VideoCapture(self.settings['recorded_video'])
self._frame_grabber_thread = None
else:
raise Exception(
'No stream type specified for {module_name}'.format(
module_name=module_name))
def _get_frame(self):
"""Get the most recent frame from the video stream."""
if self._frame_grabber_thread is not None:
self._fresh_frame_available.wait()
return (self._got_im, self._im)
else:
return self._cap.read()
def refresh_frame(self):
"""Get and store latest frame from video stream as fast as possible."""
failed_count = 0
# Assuming 30fps, 150 fails in a row means 5s of no video.
while failed_count < self.master_settings['max_failed_frames']:
self._got_im, self._im = self._cap.read()
self._fresh_frame_available.set()
self._fresh_frame_available.clear()
if not self._got_im:
failed_count += 1
else:
failed_count = 0
raise Exception('Camera sucks')
def _vision_process(self):
"""Detect obstacle details from video stream. Report findings."""
while True:
start_time = time()
got_im, im = self._get_frame()
#print 'new frame'
if self._pipe.poll() is True:
# Send the message back to parent to prove this process
# isn't frozen.
self._pipe.send(self._pipe.recv())
if got_im:
# Check if parent sent a message over the pipe.
self.processor.load_im(im)
# self.processor.hacky_display() # FIXME this needs to go away.
packet = {}
for plugin in self._plugins:
plugin.process_image(packet)
self._com.publish_message(packet)
def main(args):
com = Communicator("datafeed/raw/depth")
while True:
com.publish_message({"DEPTH": args.depth})
time.sleep(args.epoch)
def main(args):
"""Reads a keyboard press.
The resultant command is communicated over the grapevine in a
dicitonary with this format:
{"desired_offset":
{"x": 0.0,
"y": 0.0,
"z": 0.0},
"desired_orientation":
{"x": 0.0,
"y": 0.0,
"z": 0.0},
"desired_velocity":
{"x": 0.0,
"y": 0.0,
"z": 0.0}
}
The AI's job is to interpret sensor data and video information,
and from that, specify where the sub SHOULD be relative to where
it CURRENTLY is. The desired location represents a state that
should be achieved at some future time.
The vector "desired_offset" specifies the relative location of
that desired state, the "desired_orientation" specifies which
direction the submarine should be facing, and the "desired_velocity"
specifies how the submarine should be moving at that point.
This information structure intentionally doesn't allow the AI to specify
the "roll" axis or a desired rotational velocity.
One way to think about this is that the AI module needs to figure out
what the next waypoint is, and where that waypoint is relative to
where the submarine currenlty is stationed.
"""
print """Usage:
+-------+-------+-------+-------+
| q | w | e | r |
| | | | |
| QUIT |FORWARD| N/A | RISE |
++------++------++------++-----++
| a | s | d | f |
| ROTATE| | ROTATE| |
| LEFT | BACK | RIGHT | FALL |
+-------+-------+-------+-------+
"""
com = Communicator(module_name=args.module_name)
# TODO: This is a tentative structure, and it will work for
# keyboard input, but we need to discuss what information the
# AI module will have and how it will express that information.
# If nothing else, the "desired_orientation" field should probably
# use polar coordinates with radians measured relative to magnetic
# north and gravity.
# Also, the AI module will need to communicate commands for
# torpedoes, grippers, etc. Should that be communicated in the
# same dictionary?
#
# x is left/right
# y is forward/backward
# z is up/down
missive_template = {
"desired_offset": {"x": 0.0, "y": 0.0, "z": 0.0},
"desired_orientation": {"x": 0.0, "y": 0.0, "z": 0.0},
"desired_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}
}
while True:
key = ord(getch())
missive = deepcopy(missive_template)
print "{0}\r".format(chr(key)),
if key == ord('w'):
missive['desired_offset']['y'] = 9001.0
missive['desired_velocity']['y'] = 1.0
elif key == ord('a'):
missive['desired_orientation']['z'] = 3 * math.pi / 2
elif key == ord('s'):
missive['desired_offset']['y'] = -9001
missive['desired_velocity']['y'] = -1.0
elif key == ord('d'):
missive['desired_orientation']['z'] = math.pi / 2
elif key == ord('r'):
missive['desired_offset']['z'] = 9001
missive['desired_velocity']['z'] = 1.0
elif key == ord('f'):
missive['desired_offset']['z'] = -9001
missive['desired_velocity']['z'] = -1.0
elif key == ord('e'):
# missive is unchanged from the template, so this
# will turn off the motors.
pass
elif key == ord('q'):
sys.exit()
com.publish_message(missive)
def main(args):
"""Reads a keyboard press.
The resultant command is communicated over the grapevine in a
dicitonary with this format:
{"desired_offset":
{"x": 0.0,
"y": 0.0,
"z": 0.0},
"desired_orientation":
{"x": 0.0,
"y": 0.0,
"z": 0.0},
"desired_velocity":
{"x": 0.0,
"y": 0.0,
"z": 0.0}
}
The AI's job is to interpret sensor data and video information,
and from that, specify where the sub SHOULD be relative to where
it CURRENTLY is. The desired location represents a state that
should be achieved at some future time.
The vector "desired_offset" specifies the relative location of
that desired state, the "desired_orientation" specifies which
direction the submarine should be facing, and the "desired_velocity"
specifies how the submarine should be moving at that point.
This information structure intentionally doesn't allow the AI to specify
the "roll" axis or a desired rotational velocity.
One way to think about this is that the AI module needs to figure out
what the next waypoint is, and where that waypoint is relative to
where the submarine currenlty is stationed.
"""
print """Usage:
+-------+-------+-------+-------+
| q | w | e | r |
| | | | |
| QUIT |FORWARD| N/A | RISE |
++------++------++------++-----++
| a | s | d | f |
| ROTATE| | ROTATE| |
| LEFT | BACK | RIGHT | FALL |
+-------+-------+-------+-------+
"""
com = Communicator(module_name=args.module_name)
# TODO: This is a tentative structure, and it will work for
# keyboard input, but we need to discuss what information the
# AI module will have and how it will express that information.
# If nothing else, the "desired_orientation" field should probably
# use polar coordinates with radians measured relative to magnetic
# north and gravity.
# Also, the AI module will need to communicate commands for
# torpedoes, grippers, etc. Should that be communicated in the
# same dictionary?
#
# x is left/right
# y is forward/backward
# z is up/down
missive_template = {
"desired_offset": {"x": 0.0, "y": 0.0, "z": 0.0},
"desired_orientation": {"x": 0.0, "y": 0.0, "z": 0.0},
"desired_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
}
while True:
key = ord(getch())
missive = deepcopy(missive_template)
print "{0}\r".format(chr(key)),
if key == ord("w"):
missive["desired_offset"]["y"] = 9001.0
missive["desired_velocity"]["y"] = 1.0
elif key == ord("a"):
missive["desired_orientation"]["z"] = 3 * math.pi / 2
elif key == ord("s"):
missive["desired_offset"]["y"] = -9001
missive["desired_velocity"]["y"] = -1.0
elif key == ord("d"):
missive["desired_orientation"]["z"] = math.pi / 2
elif key == ord("r"):
missive["desired_offset"]["z"] = 9001
missive["desired_velocity"]["z"] = 1.0
elif key == ord("f"):
missive["desired_offset"]["z"] = -9001
missive["desired_velocity"]["z"] = -1.0
elif key == ord("e"):
# missive is unchanged from the template, so this
# will turn off the motors.
pass
elif key == ord("q"):
sys.exit()
com.publish_message(missive)
def main(args):
com = Communicator("datafeed/raw/depth")
while True:
com.publish_message({"DEPTH": args.depth})
time.sleep(args.epoch)