예제 #1
0
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)
예제 #2
0
    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()
예제 #3
0
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)
예제 #4
0
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)
예제 #5
0
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)
예제 #6
0
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)
예제 #7
0
파일: despot.py 프로젝트: pi19404/robosub-1
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)
예제 #8
0
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)
예제 #9
0
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)
예제 #10
0
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)
예제 #11
0
    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()
예제 #12
0
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)
예제 #13
0
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)
예제 #14
0
 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
예제 #15
0
 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)
예제 #16
0
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)
예제 #17
0
파일: logger.py 프로젝트: pi19404/robosub-1
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()
예제 #18
0
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)
예제 #19
0
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)
예제 #20
0
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)
예제 #21
0
 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)
예제 #22
0
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)
예제 #23
0
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()
예제 #24
0
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)
예제 #25
0
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)
예제 #26
0
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)
예제 #27
0
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)
예제 #28
0
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()
예제 #29
0
####  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
예제 #30
0
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()
예제 #31
0
파일: despot.py 프로젝트: pi19404/robosub-1
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)
예제 #32
0
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)
예제 #33
0
def main(args):
    com = Communicator("datafeed/raw/depth")
    while True:
        com.publish_message({"DEPTH": args.depth})
        time.sleep(args.epoch)
예제 #34
0
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)
예제 #35
0
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)
예제 #36
0
파일: mock.py 프로젝트: pi19404/robosub-1
def main(args):
    com = Communicator("datafeed/raw/depth")
    while True:
        com.publish_message({"DEPTH": args.depth})
        time.sleep(args.epoch)