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 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 start_liveplotter(get_var_callback):
"""
Starts a liveplotter.
The variable that is plotted is retrieved from get_var_callback.
This function returns immediately and the liveplotter quits when
the user closes it.
"""
import matplotlib.pyplot as plt
cancellation_token = Event()
global vals
vals = []
def fetch_data():
global vals
while not cancellation_token.is_set():
try:
data = get_var_callback()
except Exception as ex:
print(str(ex))
time.sleep(1)
continue
vals.append(data)
if len(vals) > num_samples:
vals = vals[-num_samples:]
time.sleep(1 / data_rate)
# TODO: use animation for better UI performance, see:
# https://matplotlib.org/examples/animation/simple_anim.html
def plot_data():
global vals
plt.ion()
# Make sure the script terminates when the user closes the plotter
def did_close(evt):
cancellation_token.set()
fig = plt.figure()
fig.canvas.mpl_connect('close_event', did_close)
while not cancellation_token.is_set():
plt.clf()
plt.plot(vals)
plt.legend(list(range(len(vals))))
fig.canvas.draw()
fig.canvas.start_event_loop(1 / plot_rate)
fetch_t = threading.Thread(target=fetch_data)
fetch_t.daemon = True
fetch_t.start()
plot_t = threading.Thread(target=plot_data)
plot_t.daemon = True
plot_t.start()
return cancellation_token
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 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")
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
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 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]
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")
help='No calibration conducted',
dest='no_calib',
action="store_true",
default=False)
args = parser.parse_args()
print("Looking for ODrive")
# odrv = odrive.find_any(serial_number=args.serial_number)
if (args.serial_number != None):
SERIAL_NUMS = [args.serial_number]
odrvs = [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 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
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()
for odrv_ctx in odrives_by_name.values():
for axis_idx, axis_ctx in enumerate(odrv_ctx.axes):
if not axis_ctx.name in args.ignore:
axes_by_name[axis_ctx.name] = axis_ctx
# Ensure mechanical couplings are valid
couplings = []
if test_rig_yaml['couplings'] is None:
test_rig_yaml['couplings'] = {}
else:
for coupling in test_rig_yaml['couplings']:
c = [axes_by_name[axis_name] for axis_name in coupling if (axis_name in axes_by_name)]
if len(c) > 1:
couplings.append(c)
app_shutdown_token = Event()
try:
for test in all_tests:
if isinstance(test, ODriveTest):
def odrv_test_thread(odrv_name):
odrv_ctx = odrives_by_name[odrv_name]
logger.notify('* running {} on {}...'.format(type(test).__name__, odrv_name))
try:
test.check_preconditions(odrv_ctx,
logger.indent(' {}: '.format(odrv_name)))
except:
raise PreconditionsNotMet()
test.run_test(odrv_ctx,
logger.indent(' {}: '.format(odrv_name)))
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")
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()
