"""
Action Panel
The Action Panel contains a set of Action's which can be activated through
one of three methods: a clickable button, a keyboard shortcut, or a ps3
controller binding (with ps3 controller button presses read through ROS).
"""
import logging
import matplotlib.gridspec as gridspec
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.widgets import Button
from visual_dynamics.gui.config import config
LOGGER = logging.getLogger(__name__)
ROS_ENABLED = False
try:
import rospkg
import roslib
import rospy
from sensor_msgs.msg import Joy
roslib.load_manifest('gps_agent_pkg')
ROS_ENABLED = True
except ImportError as e:
LOGGER.debug('Import ROS failed: %s', e)
except rospkg.common.ResourceNotFound as e:
LOGGER.debug('No gps_agent_pkg: %s', e)
class Action:
"""
An action is defined by a key (used to identify it), a name, and a function.
It is called by placing it on an matplotlib Axis object (as specified by
axis_pos), giving it a keyboard_binding, or giving it a ps3_binding.
"""
def __init__(self, key, name, func, axis_pos=None, keyboard_binding=None, ps3_binding=None):
self.key = key
self.name = name
self.func = func
self.axis_pos = axis_pos
self.kb = keyboard_binding
self.pb = ps3_binding
class ActionPanel:
def __init__(self, fig, gs, rows, cols, actions_arr):
"""
Constructs an ActionPanel assuming actions_arr is an array of
fully initialized actions.
Each action must have: key, name, func.
Each action can have: axis_pos, keyboard_binding, ps3_binding.
"""
assert len(actions_arr) <= rows*cols, 'Too many actions to put into gridspec.'
self._fig = fig
self._gs = gridspec.GridSpecFromSubplotSpec(rows, cols, subplot_spec=gs)
self._axarr = [plt.subplot(self._gs[i]) for i in range(len(actions_arr))]
# Read keyboard_bindings and ps3_bindings from config
self._actions = {action.key: action for action in actions_arr}
for key, action in self._actions.items():
if key in config['keyboard_bindings']:
action.kb = config['keyboard_bindings'][key]
if key in config['ps3_bindings']:
action.pb = config['ps3_bindings'][key]
self._buttons = None
self._initialize_buttons()
self._cid = self._fig.canvas.mpl_connect('key_press_event', self.on_key_press)
if ROS_ENABLED:
self._ps3_count = 0
rospy.Subscriber(config['ps3_topic'], Joy, self.ps3_callback)
def _initialize_buttons(self):
self._buttons = {}
for key, action in self._actions.items():
if action.axis_pos is None:
continue
button_name = '%s\n(%s)' % (action.name, action.kb)
if ROS_ENABLED and action.pb:
ps3_buttons = [config['inverted_ps3_button'][i] for i in action.pb]
button_name += '\n(%s)' % ',\n'.join(ps3_buttons)
self._buttons[key] = Button(self._axarr[action.axis_pos], button_name)
self._buttons[key].on_clicked(action.func)
def on_key_press(self, event):
if event.key in config['inverted_keyboard_bindings']:
key = config['inverted_keyboard_bindings'][event.key]
else:
key = event.key
print(event.key, key)
if key in self._actions:
self._actions[key].func()
else:
LOGGER.debug('Unrecognized keyboard input: %s', str(event.key))
# if event.key in config['inverted_keyboard_bindings']:
# key = config['inverted_keyboard_bindings'][event.key]
# if key in self._actions:
# self._actions[key].func()
# else:
# LOGGER.debug('Unrecognized keyboard input: %s', str(event.key))
def ps3_callback(self, joy_msg):
self._ps3_count += 1
if self._ps3_count % config['ps3_process_rate'] != 0:
return
buttons_pressed = tuple(np.nonzero(joy_msg.buttons)[0])
if buttons_pressed in config['permuted_inverted_ps3_bindings']:
self._actions[config['permuted_inverted_ps3_bindings'][buttons_pressed]].func()
else:
if ((len(buttons_pressed) == 1 and buttons_pressed[0] not in (
config['ps3_button']['rear_right_1'], config['ps3_button']['rear_right_1']))
or len(buttons_pressed) >= 2):
LOGGER.debug('Unrecognized ps3 controller input:\n%s',
str([config['inverted_ps3_button'][b] for b in buttons_pressed]))
