def process(self, action):
"""
:param action: action to execute, ''QAction''
:raises: when it doesn't recognice the action passed in, ''Exception''
"""
if action == self._reset_timeline:
self.timeline._timeline_frame.reset_timeline()
elif action == self._play_all:
self.timeline.toggle_play_all()
elif action == self._publish_all:
for topic in self.timeline._timeline_frame.topics:
if not self.timeline.start_publishing(topic):
break
elif action == self._publish_none:
for topic in self.timeline._timeline_frame.topics:
if not self.timeline.stop_publishing(topic):
break
elif action == self._thumbnail_show_action:
self.timeline._timeline_frame.set_renderers_active(True)
elif action == self._thumbnail_hide_action:
self.timeline._timeline_frame.set_renderers_active(False)
elif action in self._thumbnail_actions:
if self.timeline._timeline_frame.is_renderer_active(action.text()):
self.timeline._timeline_frame.set_renderer_active(
action.text(), False)
else:
self.timeline._timeline_frame.set_renderer_active(
action.text(), True)
elif action in self._topic_actions + self._type_actions:
popup_name = action.parentWidget().title() + '__' + action.text()
if popup_name not in self.timeline.popups:
frame = QWidget()
layout = QVBoxLayout()
frame.setLayout(layout)
frame.resize(640, 480)
viewer_type = action.data()
frame.setObjectName(popup_name)
view = viewer_type(self.timeline, frame)
self.timeline.popups.add(popup_name)
self.timeline.get_context().add_widget(frame)
self.timeline.add_view(action.parentWidget().title(), view,
frame)
frame.show()
elif action in self._publish_actions:
if self.timeline.is_publishing(action.text()):
self.timeline.stop_publishing(action.text())
else:
self.timeline.start_publishing(action.text())
else:
raise Exception('Unknown action in TimelinePopupMenu.process')
class ResponseVis(Plugin):
def __init__(self, context):
super(ResponseVis, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('JoyVis')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._ui = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_response_vis'), 'resource', 'plot.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._ui)
self._plotWidget = DataPlot()
self._redraw_interval = 1.0/60.0
self._ui.plot_frame.layout().addWidget(self._plotWidget)
self._ui.resp_acc_ang.toggled.connect(self.on_response_changed)
self._ui.resp_acc_lin.toggled.connect(self.on_response_changed)
self._ui.resp_vel_ang.toggled.connect(self.on_response_changed)
self._ui.resp_vel_lin.toggled.connect(self.on_response_changed)
self._ui.resp_pos_ang.toggled.connect(self.on_response_changed)
self._ui.resp_pos_lin.toggled.connect(self.on_response_changed)
self._ui.slider_zoom_ms.valueChanged.connect(self.on_width_slider_changed)
self._ui.check_pause.toggled.connect(self.on_pause_button_clicked)
self.horizon = 5.0
self._plotWidget.set_x_width(self.horizon)
# Give QObjects reasonable names
self._ui.setObjectName('ResponseVis')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._ui.setWindowTitle(self._ui.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._ui)
self._response_model = ResponseModel()
self.topics = {"odom": "/gnc/odom",
"cmd_acc": "/gnc/cmd_accel",
"cmd_vel": "/gnc/cmd_vel",
"cmd_pos": "/gnc/cmd_pose"}
self._response_model.declare_subscribers(self.topics)
self.mode = None
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._ui.show()
self.on_response_changed()
self.enable_timer()
def shutdown_plugin(self):
print("shutting down joy_vis plugin")
def save_settings(self, plugin_settings, instance_settings):
# instance_settings.set_value(k, v)
instance_settings.set_value("topics", json.dumps(self.topics))
pass
def restore_settings(self, plugin_settings, instance_settings):
# v = instance_settings.value(k)
self.topics = json.loads(instance_settings.value("topics"))
# self._response_model.declare_subscribers(self.topics)
pass
@Slot()
def update_plot(self):
if self._plotWidget is not None and self.mode is not None:
self._plotWidget.set_autoscale(x=None, y=DataPlot.SCALE_ALL)
for i in range(6):
x, y = self._response_model.get_data(ModeResponses[self.mode][i], self._redraw_interval*10)
self._plotWidget.update_values(str(i), x, y)
self._plotWidget.redraw()
@Slot()
def on_response_changed(self):
if self._ui.resp_pos_lin.isChecked():
self.mode = ResponseMode.pos_lin
if self._ui.resp_pos_ang.isChecked():
self.mode = ResponseMode.pos_ang
if self._ui.resp_vel_lin.isChecked():
self.mode = ResponseMode.vel_lin
if self._ui.resp_vel_ang.isChecked():
self.mode = ResponseMode.vel_ang
if self._ui.resp_acc_lin.isChecked():
self.mode = ResponseMode.acc_lin
if self._ui.resp_acc_ang.isChecked():
self.mode = ResponseMode.acc_ang
if self.mode is None:
return
self._plotWidget.remove_curve(0)
self._plotWidget.remove_curve(1)
self._plotWidget.remove_curve(2)
self._plotWidget.remove_curve(3)
self._plotWidget.remove_curve(4)
self._plotWidget.remove_curve(5)
x, y = self._response_model.get_data(ModeResponses[self.mode][0])
self._plotWidget.add_curve(str(0), ModeResponses[self.mode][0].name, x, y, curve_color=QColor(255, 0, 0))
x, y = self._response_model.get_data(ModeResponses[self.mode][1])
self._plotWidget.add_curve(str(1), ModeResponses[self.mode][1].name, x, y, curve_color=QColor(0, 255, 0))
x, y = self._response_model.get_data(ModeResponses[self.mode][2])
self._plotWidget.add_curve(str(2), ModeResponses[self.mode][2].name, x, y, curve_color=QColor(0, 0, 255))
x, y = self._response_model.get_data(ModeResponses[self.mode][3])
self._plotWidget.add_curve(str(3), ModeResponses[self.mode][3].name, x, y, curve_color=QColor(255, 127, 127), dashed=True)
x, y = self._response_model.get_data(ModeResponses[self.mode][4])
self._plotWidget.add_curve(str(4), ModeResponses[self.mode][4].name, x, y, curve_color=QColor(127, 255, 127), dashed=True)
x, y = self._response_model.get_data(ModeResponses[self.mode][5])
self._plotWidget.add_curve(str(5), ModeResponses[self.mode][5].name, x, y, curve_color=QColor(127, 127, 255), dashed=True)
self._plotWidget.redraw()
@Slot()
def on_pause_button_clicked(self):
self.enable_timer(not self._ui.check_pause.isChecked())
@Slot()
def on_width_slider_changed(self):
val = self._ui.slider_zoom_ms.value() / 1000.0
self._plotWidget.set_x_width(val)
self._plotWidget.redraw()
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
