From 3fbd2761aca72749bbd0f02c470f73c0c34aa429 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 17 Mar 2018 15:50:31 -0700 Subject: [PATCH] move liveplotter to odrive.utils, make explore_odrive work with IPython --- tools/explore_odrive.py | 181 ++++++++++++++++++++++------------------ tools/liveplotter.py | 43 ++-------- tools/odrive/utils.py | 142 +++++++++++++++++++++++++++++++ 3 files changed, 249 insertions(+), 117 deletions(-) create mode 100755 tools/odrive/utils.py diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py index b6120a5c..fcf3b7d0 100755 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -8,38 +8,12 @@ import sys import platform import threading import odrive.discovery +from odrive.utils import start_liveplotter # Flush stdout by default import functools print = functools.partial(print, flush=True) -# Check if IPython is installed -try: - import IPython - embed_ipython = True -except: - embed_ipython = False - - print("Warning: you don't have IPython installed.") - print("If you want to have an improved interactive console with pretty colors,") - print("you should install IPython\n") - - # Ensure interactive mode - if not bool(getattr(sys, 'ps1', sys.flags.interactive)): - print("You're not running in interactive mode. Run python -i explore_odrive.py") - print('') - sys.exit(1) - - # Enable tab complete if possible - try: - import readline - readline.parse_and_bind("tab: complete") - except: - sudo_prefix = "" if platform.system() == "Windows" else "sudo " - print("Warning: could not enable tab-complete. User experience will suffer.\n" - "Run `{}pip install readline` and then restart this script to fix this." - .format(sudo_prefix)) - # some enums described in the README # TODO: transmit as part of the JSON @@ -53,7 +27,7 @@ CTRL_MODE_VELOCITY_CONTROL = 2, CTRL_MODE_POSITION_CONTROL = 3 -# Parse arguments +## Parse arguments ## parser = argparse.ArgumentParser(description='Load an odrive object to play with in the IPython interactive shell.', formatter_class=argparse.RawTextHelpFormatter) parser.add_argument("-v", "--verbose", action="store_true", @@ -72,6 +46,10 @@ parser.add_argument("-p", "--path", metavar="PATH", action="store", "Example:\n" " --path usb,serial:/dev/ttyUSB0\n" "means \"discover any USB device or a serial device on /dev/ttyUSB0\"") +parser.add_argument("--no-ipython", action="store_true", + help="Use the regular Python shell\n" + "instead of the IPython shell,\n" + "even if IPython is installed\n") parser.add_argument("-s", "--serial-number", action="store", help="The serial number of the device. If omitted, any device is accepted.\n") parser.set_defaults(path="usb") @@ -82,11 +60,60 @@ if (args.verbose): else: printer = lambda x: None + +## Interactive console utils ## + COLOR_RED = '\x1b[91;1m' COLOR_CYAN = '\x1b[96;1m' COLOR_RESET = '\x1b[0m' +def print_on_second_last_line(text, **kwargs): + """ + Prints a text on the second last line. + This can be used to print a message above the command + prompt. If the command prompt spans multiple lines, + there will be glitches. + """ + # Escape character sequence: + # ESC 7: store cursor position + # ESC 1A: move cursor up by one + # ESC 1S: scroll entire viewport by one + # ESC 1L: insert 1 line at cursor position + # (print text) + # ESC 8: restore old cursor position + kwargs['end'] = '' + kwargs['flush'] = True + print('\x1b7\x1b[1A\x1b[1S\x1b[1L' + text + '\x1b8', **kwargs) + +def print_banner(): + print('ODrive control utility v0.4') + print('Please connect your ODrive.') + print('Type help() for help.') + +def print_help(): + print('') + if len(discovered_devices) == 0: + print('Connect your ODrive to {} and power it up.'.format(args.path)) + print('After that, the following message should appear:') + print(' "Connected to ODrive [serial number] as odrv0"') + print('') + print('Once the ODrive is connected, type "odrv0." and press ') + else: + print('Type "odrv0." and press ') + print('This will present you with all the properties that you can reference') + print('') + print('For example: "odrv0.motor0.encoder.pll_pos"') + print('will print the current encoder position on motor 0') + print('and "odrv0.motor0.pos_setpoint = 10000"') + print('will send motor0 to 10000') + print('') + interactive_variables = {} +interactive_variables["help"] = print_help + + +## Device discovery ## + discovered_devices = [] def did_discover_device(odrive): @@ -130,61 +157,55 @@ odrive.discovery.find_all(args.path, args.serial_number, printer=printer) -def print_help(): - print('') - print('ODRIVE EXPLORER') - print('') - print('Type "odrv0." and press ') - print('This will present you with all the properties that you can reference') - print('') - print('For example: "odrv0.motor0.encoder.pll_pos"') - print('will print the current encoder position on motor 0') - print('and "odrv0.motor0.pos_setpoint = 10000"') - print('will send motor0 to 10000') - print('') +## Launch interactive shell ## -def print_on_second_last_line(text, **kwargs): - """ - Prints a text on the second last line. - This can be used to print a message above the command - prompt. If the command prompt spans multiple lines, - there will be glitches. - """ - # Escape character sequence: - # ESC 7: store cursor position - # ESC 1A: move cursor up by one - # ESC 1S: scroll entire viewport by one - # ESC 1L: insert 1 line at cursor position - # (print text) - # ESC 8: restore old cursor position - kwargs['end'] = '' - kwargs['flush'] = True - print('\x1b7\x1b[1A\x1b[1S\x1b[1L' + text + '\x1b8', **kwargs) - - -embed_ipython = False +# Check if IPython is installed +if args.no_ipython: + use_ipython = False +else: + try: + import IPython + use_ipython = True + except: + print("Warning: you don't have IPython installed.") + print("If you want to have an improved interactive console with pretty colors,") + print("you should install IPython\n") + use_ipython = False # If IPython is installed, embed IPython shell, otherwise embed regular shell -if embed_ipython: - IPython.embed() +if use_ipython: + #interactive_variables["lalala"] = print_help + help = print_help # Override help function + console = IPython.terminal.embed.InteractiveShellEmbed(local_ns=interactive_variables, banner1='') + console.runcode = console.run_code # hack to make IPython look like the regular console + interact = console else: - import code - import rlcompleter - import readline - readline.parse_and_bind("tab: complete") - console = code.InteractiveConsole(locals=interactive_variables) - console.locals["help"] = print_help - console.runcode('import sys') - console.runcode('superexcepthook = sys.excepthook') - console.runcode('def newexcepthook(ex_class,ex,trace):\n' - ' if ex_class.__module__ == "odrive.protocol" and ex_class.__name__ == "ChannelBrokenException":\n' - ' pass\n' - ' else:\n' - ' superexcepthook(ex_class,ex,trace)') - console.runcode('sys.excepthook=newexcepthook') - #print = print_on_second_last_line - console.interact(banner='ODrive control utility v0.4\n' - 'Please connect your ODrive.\n' - 'Type help() for help.') + # Enable tab complete if possible + try: + import rlcompleter + import readline + readline.parse_and_bind("tab: complete") + except: + sudo_prefix = "" if platform.system() == "Windows" else "sudo " + print("Warning: could not enable tab-complete. User experience will suffer.\n" + "Run `{}pip install readline` and then restart this script to fix this." + .format(sudo_prefix)) + + import code + console = code.InteractiveConsole(locals=interactive_variables) + interact = lambda: console.interact(banner='') + +# install hook to hide ChannelBrokenException +console.runcode('import sys') +console.runcode('superexcepthook = sys.excepthook') +console.runcode('def newexcepthook(ex_class,ex,trace):\n' + ' if ex_class.__module__ + "." + ex_class.__name__ != "odrive.protocol.ChannelBrokenException":\n' + ' superexcepthook(ex_class,ex,trace)') +console.runcode('sys.excepthook=newexcepthook') + + +# Launch shell +print_banner() +interact() app_shutdown_token.set() diff --git a/tools/liveplotter.py b/tools/liveplotter.py index e67ca8cf..95c50c24 100755 --- a/tools/liveplotter.py +++ b/tools/liveplotter.py @@ -4,10 +4,9 @@ Liveplotter """ import time -import odrive.discovery -import matplotlib.pyplot as plt -import numpy as np import threading +import odrive.discovery +from odrive.utils import start_liveplotter data_rate = 100 plot_rate = 10 @@ -15,38 +14,8 @@ num_samples = 1000 my_odrive = odrive.discovery.find_any() -plt.ion() -global vals -vals = [] +# If you want to plot different values, change them here. +# You can plot any number of values concurrently. +start_liveplotter(lambda: [my_odrive.motor0.encoder.pll_pos, + my_odrive.motor1.encoder.pll_pos]) -# Make sure the script terminates when the user closes the plotter -cancellation_token = threading.Event() -def handle_close(evt): - cancellation_token.set() -fig = plt.figure() -fig.canvas.mpl_connect('close_event', handle_close) - -def fetch_data(): - global vals - global cancellation_token - while not cancellation_token.is_set(): - vals.append(my_odrive.motor0.encoder.pll_pos) - 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 - global cancellation_token - while not cancellation_token.is_set(): - plt.clf() - plt.plot(vals) - #time.sleep(1/plot_rate) - fig.canvas.flush_events() - -fetch_thread = threading.Thread(target=fetch_data, daemon=True) -fetch_thread.start() - -plot_data() diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py new file mode 100755 index 00000000..f1fb779b --- /dev/null +++ b/tools/odrive/utils.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 +""" +Liveplotter +""" + +import sys +import time +import threading + +data_rate = 100 +plot_rate = 10 +num_samples = 1000 + +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 + import numpy as np + + cancellation_token = threading.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) + #time.sleep(1/plot_rate) + fig.canvas.flush_events() + + threading.Thread(target=fetch_data).start() + threading.Thread(target=plot_data).start() + #plot_data() + +## Exceptions ## + +class TimeoutException(Exception): + pass + +## Threading utils ## + +class Event(): + """ + Alternative to threading.Event(), enhanced by the subscribe() function + that the original fails to provide. + """ + def __init__(self): + self._evt = threading.Event() + self._subscribers = [] + self._mutex = threading.Lock() + + def is_set(self): + return self._evt.is_set() + + def set(self): + """ + Sets the event and invokes all subscribers if the event was + not already set + """ + self._mutex.acquire() + try: + if not self._evt.is_set(): + self._evt.set() + for s in self._subscribers: + s() + finally: + self._mutex.release() + + def subscribe(self, handler): + """ + Invokes the specified handler exactly once as soon as the + specified event is set. If the event is already set, the + handler is invoked immediately. + Returns a function that can be invoked to unsubscribe. + """ + self._mutex.acquire() + try: + self._subscribers.append(handler) + if self._evt.is_set(): + handler() + finally: + self._mutex.release() + return lambda: self.unsubscribe(handler) + + def unsubscribe(self, handler): + self._mutex.acquire() + try: + self._subscribers.pop(self._subscribers.index(handler)) + finally: + self._mutex.release() + + def wait(self, timeout=None): + return self._evt.wait(timeout=timeout) + +def wait_any(*events, timeout=None): + """ + Blocks until any of the specified events are triggered. + Returns the number of the event that was triggerd or raises + a TimeoutException + """ + or_event = threading.Event() + unsubscribe_functions = [] + for event in events: + unsubscribe_functions.append(event.subscribe(lambda: or_event.set())) + or_event.wait(timeout=timeout) + for unsubscribe_function in unsubscribe_functions: + unsubscribe_function() + for i in range(len(events)): + if events[i].is_set(): + return i + raise TimeoutException()