mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-09-21 15:34:33 +08:00
move liveplotter to odrive.utils, make explore_odrive work with IPython
This commit is contained in:
Samuel Sadok
+101
-80
@@ -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 <tab>')
|
||||
else:
|
||||
print('Type "odrv0." and press <tab>')
|
||||
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 <tab>')
|
||||
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()
|
||||
|
||||
+6
-37
@@ -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()
|
||||
|
||||
Executable
+142
@@ -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()
|
||||
Reference in New Issue
Block a user