Exemple #1
0
def find_any(path="usb", serial_number=None,
        search_cancellation_token=None, channel_termination_token=None,
        timeout=None, logger=Logger(verbose=False), find_multiple=False):
    """
    Blocks until the first matching Fibre object is connected and then returns that object
    """
    result = []
    done_signal = Event(search_cancellation_token)
    def did_discover_object(obj):
        result.append(obj)
        if find_multiple:
            if len(result) >= int(find_multiple):
               done_signal.set()
        else:
            done_signal.set()

    find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)

    try:
        done_signal.wait(timeout=timeout)
    except TimeoutError:
        if not find_multiple:
            return None
    finally:
        done_signal.set() # terminate find_all

    if find_multiple:
        return result
    else:
        return result[0] if len(result) > 0 else None
    def get_odrives(self):
        self.odrvs = [None, None, None]

        # Get ODrives
        done_signal = Event(None)

        def discovered_odrv(obj):
            print("Found odrive with sn: {}".format(obj.serial_number))
            if obj.serial_number in self.SERIAL_NUMS:
                self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
                print("ODrive is # {}".format(
                    self.SERIAL_NUMS.index(obj.serial_number)))
            else:
                print("ODrive sn not found in list. New ODrive?")
            if not None in self.odrvs:
                done_signal.set()

        odrive.find_all("usb", None, discovered_odrv, done_signal, None,
                        Logger(verbose=False))
        # Wait for ODrives
        try:
            done_signal.wait(timeout=120)
        finally:
            done_signal.set()

        rospy.loginfo("Found ODrives")
Exemple #3
0
    def find_all_odrives(self,
                         path="usb",
                         serial_number=None,
                         search_cancellation_token=None,
                         channel_termination_token=None,
                         timeout=30,
                         logger=Logger(verbose=True)):
        """
        Blocks until timeout
        """
        result = []
        done_signal = Event(search_cancellation_token)
        self.drive_count = 0

        def did_discover_object(obj):
            result.append(obj)
            self.drive_count += 1

        odrive.find_all(path, serial_number, did_discover_object, done_signal,
                        channel_termination_token, logger)
        try:
            done_signal.wait(timeout=timeout)

        except TimeoutError:
            print("Timeouted")
        finally:
            done_signal.set()  # terminate find_all
        return result
Exemple #4
0
def find_any(path="usb", serial_number=None,
        search_cancellation_token=None, channel_termination_token=None,
        timeout=None, logger=Logger(verbose=False)):
    """
    Blocks until the first matching Fibre node is connected and then returns that node
    """
    result = [ None ]
    done_signal = Event(search_cancellation_token)
    def did_discover_object(obj):
        result[0] = obj
        done_signal.set()
    find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)
    try:
        done_signal.wait(timeout=timeout)
    finally:
        done_signal.set() # terminate find_all
    return result[0]
Exemple #5
0
class Channel(PacketSink):
    # Choose these parameters to be sensible for a specific transport layer
    _resend_timeout = 5.0  # [s]
    _send_attempts = 5

    def __init__(self, name, input, output, cancellation_token, logger):
        """
        Params:
        input: A PacketSource where this channel will source packets from on
               demand. Alternatively packets can be provided to this channel
               directly by calling process_packet on this instance.
        output: A PacketSink where this channel will put outgoing packets.
        """
        self._name = name
        self._input = input
        self._output = output
        self._logger = logger
        self._outbound_seq_no = 0
        self._interface_definition_crc = 0
        self._expected_acks = {}
        self._responses = {}
        self._my_lock = threading.Lock()
        self._channel_broken = Event(cancellation_token)
        self.start_receiver_thread(Event(self._channel_broken))

    def start_receiver_thread(self, cancellation_token):
        """
        Starts the receiver thread that processes incoming messages.
        The thread quits as soon as the channel enters a broken state.
        """
        def receiver_thread():
            error_ctr = 0
            try:
                while not cancellation_token.is_set():
                    if error_ctr >= 10:
                        raise ChannelBrokenException(
                            "too many consecutive receive errors")
                    # Set an arbitrary deadline because the get_packet function
                    # currently doesn't support a cancellation_token
                    deadline = time.monotonic() + 1.0
                    try:
                        response = self._input.get_packet(deadline)
                    except TimeoutError:
                        continue  # try again
                    except ChannelDamagedException:
                        error_ctr += 1
                        continue  # try again
                    if (error_ctr > 0):
                        error_ctr -= 1
                    # Process response
                    # This should not throw an exception, otherwise the channel breaks
                    self.process_packet(response)
                #print("receiver thread is exiting")
            except Exception:
                self._logger.debug("receiver thread is exiting: " +
                                   traceback.format_exc())
            finally:
                self._channel_broken.set("recv thread died")

        threading.Thread(name='fibre-receiver', target=receiver_thread).start()

    def remote_endpoint_operation(self, endpoint_id, input, expect_ack,
                                  output_length):
        if input is None:
            input = bytearray(0)
        if (len(input) >= 128):
            raise Exception("packet larger than 127 currently not supported")

        if (expect_ack):
            endpoint_id |= 0x8000

        self._my_lock.acquire()
        try:
            self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff)
            seq_no = self._outbound_seq_no
        finally:
            self._my_lock.release()
        seq_no |= 0x80  # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the ascii protocol
        packet = struct.pack('<HHH', seq_no, endpoint_id, output_length)
        packet = packet + input

        crc16 = calc_crc16(CRC16_INIT, packet)
        if (endpoint_id & 0x7fff == 0):
            trailer = PROTOCOL_VERSION
        else:
            trailer = self._interface_definition_crc
        #print("append trailer " + trailer)
        packet = packet + struct.pack('<H', trailer)

        if (expect_ack):
            ack_event = Event()
            self._expected_acks[seq_no] = ack_event
            try:
                attempt = 0
                while (attempt < self._send_attempts):
                    self._my_lock.acquire()
                    try:
                        self._output.process_packet(packet)
                    except ChannelDamagedException:
                        attempt += 1
                        continue  # resend
                    finally:
                        self._my_lock.release()
                    # Wait for ACK until the resend timeout is exceeded
                    try:
                        if wait_any(self._resend_timeout, ack_event,
                                    self._channel_broken) != 0:
                            raise ChannelBrokenException()
                    except TimeoutError:
                        attempt += 1
                        continue  # resend
                    return self._responses.pop(seq_no)
                    # TODO: record channel statistics
                raise ChannelBrokenException()  # Too many resend attempts
            finally:
                self._expected_acks.pop(seq_no)
                self._responses.pop(seq_no, None)
        else:
            # fire and forget
            self._output.process_packet(packet)
            return None

    def remote_endpoint_read_buffer(self, endpoint_id):
        """
        Handles reads from long endpoints
        """
        # TODO: handle device that could (maliciously) send infinite stream
        buffer = bytes()
        while True:
            chunk_length = 512
            chunk = self.remote_endpoint_operation(
                endpoint_id, struct.pack("<I", len(buffer)), True,
                chunk_length)
            if (len(chunk) == 0):
                break
            buffer += chunk
        return buffer

    def process_packet(self, packet):
        #print("process packet")
        packet = bytes(packet)
        if (len(packet) < 2):
            raise Exception("packet too short")

        seq_no = struct.unpack('<H', packet[0:2])[0]

        if (seq_no & 0x8000):
            seq_no &= 0x7fff
            ack_signal = self._expected_acks.get(seq_no, None)
            if (ack_signal):
                self._responses[seq_no] = packet[2:]
                ack_signal.set("ack")
                #print("received ack for packet " + str(seq_no))
            else:
                print("received unexpected ACK: " + str(seq_no))

        else:
            #if (calc_crc16(CRC16_INIT, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):
            #     raise Exception("CRC16 mismatch")
            print("endpoint requested")
        if obj.serial_number in SERIAL_NUMS:
            odrvs[SERIAL_NUMS.index(obj.serial_number)] = obj
            print("ODrive is # {}".format(SERIAL_NUMS.index(
                obj.serial_number)))
        else:
            print("ODrive sn not found in list. New ODrive?")
        if not None in odrvs:
            done_signal.set()

    odrive.find_all("usb", None, discovered_odrv, done_signal, None,
                    Logger(verbose=False))
    # Wait for ODrives
    try:
        done_signal.wait(timeout=120)
    finally:
        done_signal.set()

    # Which odrives
    if args.which_odrive == None:
        to_calib = odrvs
    else:
        to_calib = [odrvs[args.which_odrive]]

    for odrv in to_calib:
        odrv.config.brake_resistance = 0.5
        print("Calibrating ODrive # {}".format(to_calib.index(odrv)))
        if args.calib_both_axis:
            odrv.axis0.watchdog_feed()
            odrv.axis1.watchdog_feed()
            clear_errors(odrv)
            odrv.erase_configuration()
Exemple #7
0
class BulkCapture:
    '''
    Asynchronously captures a bulk set of data when instance is created.

    get_var_callback: a function that returns the data you want to collect (see the example below)
    data_rate: Rate in hz
    length: Length of time to capture in seconds

    Example Usage:
        capture = BulkCapture(lambda :[odrv0.axis0.encoder.pos_estimate, odrv0.axis0.controller.pos_setpoint])
        # Do stuff while capturing (like sending position commands)
        capture.event.wait() # When you're done doing stuff, wait for the capture to be completed.
        print(capture.data) # Do stuff with the data
        capture.plot_data() # Helper method to plot the data
    '''
    def __init__(self, get_var_callback, data_rate=500.0, duration=2.0):
        from threading import Event, Thread
        import numpy as np

        self.get_var_callback = get_var_callback
        self.event = Event()

        def loop():
            vals = []
            start_time = time.monotonic()
            period = 1.0 / data_rate
            while time.monotonic() - start_time < duration:
                try:
                    data = get_var_callback()
                except Exception as ex:
                    print(str(ex))
                    print("Waiting 1 second before next data point")
                    time.sleep(1)
                    continue
                relative_time = time.monotonic() - start_time
                vals.append([relative_time] + data)
                time.sleep(period -
                           (relative_time %
                            period))  # this ensures consistently timed samples
            self.data = np.array(
                vals)  # A lock is not really necessary due to the event
            print("Capture complete")
            achieved_data_rate = len(self.data) / self.data[-1, 0]
            if achieved_data_rate < (data_rate * 0.9):
                print("Achieved average data rate: {}Hz".format(
                    achieved_data_rate))
                print(
                    "If this rate is significantly lower than what you specified, consider lowering it below the achieved value for more consistent sampling."
                )
            self.event.set(
            )  # tell the main thread that the bulk capture is complete

        Thread(target=loop, daemon=True).start()

    def plot(self):
        import matplotlib.pyplot as plt
        import inspect
        from textwrap import wrap
        plt.plot(self.data[:, 0], self.data[:, 1:])
        plt.xlabel("Time (seconds)")
        title = (str(inspect.getsource(
            self.get_var_callback)).strip("['\\n']").split(" = ")[1])
        plt.title("\n".join(wrap(title, 60)))
        plt.legend(range(self.data.shape[1] - 1))
        plt.show()
Exemple #8
0
class Channel(PacketSink):
    # Choose these parameters to be sensible for a specific transport layer
    _resend_timeout = 5.0     # [s]
    _send_attempts = 5

    def __init__(self, name, input, output, cancellation_token, logger):
        """
        Params:
        input: A PacketSource where this channel will source packets from on
               demand. Alternatively packets can be provided to this channel
               directly by calling process_packet on this instance.
        output: A PacketSink where this channel will put outgoing packets.
        """
        self._name = name
        self._input = input
        self._output = output
        self._logger = logger
        self._outbound_seq_no = 0
        self._interface_definition_crc = 0
        self._expected_acks = {}
        self._responses = {}
        self._my_lock = threading.Lock()
        self._channel_broken = Event(cancellation_token)
        self.start_receiver_thread(Event(self._channel_broken))

    def start_receiver_thread(self, cancellation_token):
        """
        Starts the receiver thread that processes incoming messages.
        The thread quits as soon as the channel enters a broken state.
        """
        def receiver_thread():
            error_ctr = 0
            try:
                while (not cancellation_token.is_set() and not self._channel_broken.is_set()
                        and error_ctr < 10):
                    # Set an arbitrary deadline because the get_packet function
                    # currently doesn't support a cancellation_token
                    deadline = time.monotonic() + 1.0
                    try:
                        response = self._input.get_packet(deadline)
                    except TimeoutError:
                        continue # try again
                    except ChannelDamagedException:
                        error_ctr += 1
                        continue # try again
                    if (error_ctr > 0):
                        error_ctr -= 1
                    # Process response
                    # This should not throw an exception, otherwise the channel breaks
                    self.process_packet(response)
                #print("receiver thread is exiting")
            except Exception:
                self._logger.debug("receiver thread is exiting: " + traceback.format_exc())
            finally:
                self._channel_broken.set()
        t = threading.Thread(target=receiver_thread)
        t.daemon = True
        t.start()

    def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length):
        if input is None:
            input = bytearray(0)
        if (len(input) >= 128):
            raise Exception("packet larger than 127 currently not supported")

        if (expect_ack):
            endpoint_id |= 0x8000

        self._my_lock.acquire()
        try:
            self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff)
            seq_no = self._outbound_seq_no
        finally:
            self._my_lock.release()
        seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the ascii protocol
        packet = struct.pack('<HHH', seq_no, endpoint_id, output_length)
        packet = packet + input

        crc16 = calc_crc16(CRC16_INIT, packet)
        if (endpoint_id & 0x7fff == 0):
            trailer = PROTOCOL_VERSION
        else:
            trailer = self._interface_definition_crc
        #print("append trailer " + trailer)
        packet = packet + struct.pack('<H', trailer)

        if (expect_ack):
            ack_event = Event()
            self._expected_acks[seq_no] = ack_event
            try:
                attempt = 0
                while (attempt < self._send_attempts):
                    self._my_lock.acquire()
                    try:
                        self._output.process_packet(packet)
                    except ChannelDamagedException:
                        attempt += 1
                        continue # resend
                    except TimeoutError:
                        attempt += 1
                        continue # resend
                    finally:
                        self._my_lock.release()
                    # Wait for ACK until the resend timeout is exceeded
                    try:
                        if wait_any(self._resend_timeout, ack_event, self._channel_broken) != 0:
                            raise ChannelBrokenException()
                    except TimeoutError:
                        attempt += 1
                        continue # resend
                    return self._responses.pop(seq_no)
                    # TODO: record channel statistics
                raise ChannelBrokenException() # Too many resend attempts
            finally:
                self._expected_acks.pop(seq_no)
                self._responses.pop(seq_no, None)
        else:
            # fire and forget
            self._output.process_packet(packet)
            return None
    
    def remote_endpoint_read_buffer(self, endpoint_id):
        """
        Handles reads from long endpoints
        """
        # TODO: handle device that could (maliciously) send infinite stream
        buffer = bytes()
        while True:
            chunk_length = 512
            chunk = self.remote_endpoint_operation(endpoint_id, struct.pack("<I", len(buffer)), True, chunk_length)
            if (len(chunk) == 0):
                break
            buffer += chunk
        return buffer

    def process_packet(self, packet):
        #print("process packet")
        packet = bytes(packet)
        if (len(packet) < 2):
            raise Exception("packet too short")

        seq_no = struct.unpack('<H', packet[0:2])[0]

        if (seq_no & 0x8000):
            seq_no &= 0x7fff
            ack_signal = self._expected_acks.get(seq_no, None)
            if (ack_signal):
                self._responses[seq_no] = packet[2:]
                ack_signal.set()
                #print("received ack for packet " + str(seq_no))
            else:
                print("received unexpected ACK: " + str(seq_no))

        else:
            #if (calc_crc16(CRC16_INIT, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):
            #     raise Exception("CRC16 mismatch")
            print("endpoint requested")
Exemple #9
0
    def __init__(self, timeout):
        
        # specify left, middle, and right ODrives
        rospy.loginfo("Looking for ODrives...")

        self.SERIAL_NUMS = [
            35593293288011,                  # Left, 0
            35623406809166,                  # Middle, 1
            35563278839886]                  # Right, 2

        self.odrvs = [
            None,
            None,
            None]

        # Get ODrives
        done_signal = Event(None)

        def discovered_odrv(obj):
            print("Found odrive with sn: {}".format(obj.serial_number))
            if obj.serial_number in self.SERIAL_NUMS:
                self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
                print("ODrive is # {}".format(self.SERIAL_NUMS.index(obj.serial_number)))
            else:
                print("ODrive sn not found in list. New ODrive?")
            if not None in self.odrvs:
                done_signal.set()

        odrive.find_all("usb", None, discovered_odrv, done_signal, None, Logger(verbose=False))
        # Wait for ODrives
        try:
            done_signal.wait(timeout=120)
        finally:
            done_signal.set()

        # self.odrv0 = odrive.find_any()
        # # odrv1 = odrive.find_any()
        # # odrv2 = odrive.find_any()
        rospy.loginfo("Found ODrives")

        # Set left and right axis
        self.leftAxes = [self.odrvs[0].axis0, self.odrvs[0].axis1, self.odrvs[1].axis1]
        self.rightAxes = [self.odrvs[1].axis0, self.odrvs[2].axis0, self.odrvs[2].axis1]
        self.axes = self.leftAxes + self.rightAxes

        # Set axis state
        rospy.logdebug("Enabling Watchdog")
        for ax in (self.leftAxes + self.rightAxes):
            ax.watchdog_feed()
            ax.config.watchdog_timeout = 2
            ax.encoder.config.ignore_illegal_hall_state = True

        # Clear errors
        for odrv in self.odrvs:
            dump_errors(odrv, True)

        for ax in (self.leftAxes + self.rightAxes):
            ax.controller.vel_ramp_enable = True
            ax.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
            ax.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL

        # Sub to topic
        rospy.Subscriber('joy', Joy, self.vel_callback)

        # Set first watchdog
        self.timeout = timeout  # log error if this many seconds occur between received messages
        self.timer = rospy.Timer(rospy.Duration(self.timeout), self.watchdog_callback, oneshot=True)
        self.watchdog_fired = False

        # Init other variables
        self.last_msg_time = 0
        self.last_recv_time = 0
        self.next_wd_feed_time = 0

        rospy.loginfo("Ready for topic")
        rospy.spin()