def __init__(self, timeline, parent, topic):
super(PlotWidget, self).__init__(parent)
self.setObjectName('PlotWidget')
self.timeline = timeline
msg_type = self.timeline.get_datatype(topic)
self.msgtopic = topic
self.start_stamp = self.timeline._get_start_stamp()
self.end_stamp = self.timeline._get_end_stamp()
# the current region-of-interest for our bag file
# all resampling and plotting is done with these limits
self.limits = [0,(self.end_stamp-self.start_stamp).to_sec()]
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_bag_plugins'), 'resource', 'plot.ui')
loadUi(ui_file, self)
self.message_tree = MessageTree(msg_type, self)
self.data_tree_layout.addWidget(self.message_tree)
# TODO: make this a dropdown with choices for "Auto", "Full" and
# "Custom"
# I continue to want a "Full" option here
self.auto_res.stateChanged.connect(self.autoChanged)
self.resolution.editingFinished.connect(self.settingsChanged)
self.resolution.setValidator(QDoubleValidator(0.0,1000.0,6,self.resolution))
self.timeline.selected_region_changed.connect(self.region_changed)
self.recompute_timestep()
self.plot = DataPlot(self)
self.plot.set_autoscale(x=False)
self.plot.set_autoscale(y=DataPlot.SCALE_VISIBLE)
self.plot.autoscroll(False)
self.plot.set_xlim(self.limits)
self.data_plot_layout.addWidget(self.plot)
self._home_button = QPushButton()
self._home_button.setToolTip("Reset View")
self._home_button.setIcon(QIcon.fromTheme('go-home'))
self._home_button.clicked.connect(self.home)
self.plot_toolbar_layout.addWidget(self._home_button)
self._config_button = QPushButton("Configure Plot")
self._config_button.clicked.connect(self.plot.doSettingsDialog)
self.plot_toolbar_layout.addWidget(self._config_button)
self.set_cursor(0)
self.paths_on = set()
self._lines = None
# get bag from timeline
bag = None
start_time = self.start_stamp
while bag is None:
bag,entry = self.timeline.get_entry(start_time, topic)
if bag is None:
start_time = self.timeline.get_entry_after(start_time)[1].time
self.bag = bag
# get first message from bag
msg = bag._read_message(entry.position)
self.message_tree.set_message(msg[1])
# state used by threaded resampling
self.resampling_active = False
self.resample_thread = None
self.resample_fields = set()
class PlotWidget(QWidget):
def __init__(self, timeline, parent, topic):
super(PlotWidget, self).__init__(parent)
self.setObjectName('PlotWidget')
self.timeline = timeline
msg_type = self.timeline.get_datatype(topic)
self.msgtopic = topic
self.start_stamp = self.timeline._get_start_stamp()
self.end_stamp = self.timeline._get_end_stamp()
# the current region-of-interest for our bag file
# all resampling and plotting is done with these limits
self.limits = [0,(self.end_stamp-self.start_stamp).to_sec()]
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_bag_plugins'), 'resource', 'plot.ui')
loadUi(ui_file, self)
self.message_tree = MessageTree(msg_type, self)
self.data_tree_layout.addWidget(self.message_tree)
# TODO: make this a dropdown with choices for "Auto", "Full" and
# "Custom"
# I continue to want a "Full" option here
self.auto_res.stateChanged.connect(self.autoChanged)
self.resolution.editingFinished.connect(self.settingsChanged)
self.resolution.setValidator(QDoubleValidator(0.0,1000.0,6,self.resolution))
self.timeline.selected_region_changed.connect(self.region_changed)
self.recompute_timestep()
self.plot = DataPlot(self)
self.plot.set_autoscale(x=False)
self.plot.set_autoscale(y=DataPlot.SCALE_VISIBLE)
self.plot.autoscroll(False)
self.plot.set_xlim(self.limits)
self.data_plot_layout.addWidget(self.plot)
self._home_button = QPushButton()
self._home_button.setToolTip("Reset View")
self._home_button.setIcon(QIcon.fromTheme('go-home'))
self._home_button.clicked.connect(self.home)
self.plot_toolbar_layout.addWidget(self._home_button)
self._config_button = QPushButton("Configure Plot")
self._config_button.clicked.connect(self.plot.doSettingsDialog)
self.plot_toolbar_layout.addWidget(self._config_button)
self.set_cursor(0)
self.paths_on = set()
self._lines = None
# get bag from timeline
bag = None
start_time = self.start_stamp
while bag is None:
bag,entry = self.timeline.get_entry(start_time, topic)
if bag is None:
start_time = self.timeline.get_entry_after(start_time)[1].time
self.bag = bag
# get first message from bag
msg = bag._read_message(entry.position)
self.message_tree.set_message(msg[1])
# state used by threaded resampling
self.resampling_active = False
self.resample_thread = None
self.resample_fields = set()
def set_cursor(self, position):
self.plot.vline(position, color=DataPlot.RED)
self.plot.redraw()
def add_plot(self, path):
self.resample_data([path])
def update_plot(self):
if len(self.paths_on)>0:
self.resample_data(self.paths_on)
def remove_plot(self, path):
self.plot.remove_curve(path)
self.paths_on.remove(path)
self.plot.redraw()
def load_data(self):
"""get a generator for the specified time range on our bag"""
return self.bag.read_messages(self.msgtopic,
self.start_stamp+rospy.Duration.from_sec(self.limits[0]),
self.start_stamp+rospy.Duration.from_sec(self.limits[1]))
def resample_data(self, fields):
if self.resample_thread:
# cancel existing thread and join
self.resampling_active = False
self.resample_thread.join()
for f in fields:
self.resample_fields.add(f)
# start resampling thread
self.resampling_active = True
self.resample_thread = threading.Thread(target=self._resample_thread)
# explicitly mark our resampling thread as a daemon, because we don't
# want to block program exit on a long resampling operation
self.resample_thread.setDaemon(True)
self.resample_thread.start()
def _resample_thread(self):
# TODO:
# * look into doing partial display updates for long resampling
# operations
# * add a progress bar for resampling operations
x = {}
y = {}
for path in self.resample_fields:
x[path] = []
y[path] = []
msgdata = self.load_data()
for entry in msgdata:
# detect if we're cancelled and return early
if not self.resampling_active:
return
for path in self.resample_fields:
# this resampling method is very unstable, because it picks
# representative points rather than explicitly representing
# the minimum and maximum values present within a sample
# If the data has spikes, this is particularly bad because they
# will be missed entirely at some resolutions and offsets
if x[path]==[] or (entry[2]-self.start_stamp).to_sec()-x[path][-1] >= self.timestep:
y_value = entry[1]
for field in path.split('.'):
index = None
if field.endswith(']'):
field = field[:-1]
field, _, index = field.rpartition('[')
y_value = getattr(y_value, field)
if index:
index = int(index)
y_value = y_value[index]
y[path].append(y_value)
x[path].append((entry[2]-self.start_stamp).to_sec())
# TODO: incremental plot updates would go here...
# we should probably do incremental updates based on time;
# that is, push new data to the plot maybe every .5 or .1
# seconds
# time is a more useful metric than, say, messages loaded or
# percentage, because it will give a reasonable refresh rate
# without overloading the computer
# if we had a progress bar, we could emit a signal to update it here
# update the plot with final resampled data
for path in self.resample_fields:
if len(x[path]) < 1:
qWarning("Resampling resulted in 0 data points for %s" % path)
else:
if path in self.paths_on:
self.plot.clear_values(path)
self.plot.update_values(path, x[path], y[path])
else:
self.plot.add_curve(path, path, x[path], y[path])
self.paths_on.add(path)
self.plot.redraw()
self.resample_fields.clear()
self.resampling_active = False
def recompute_timestep(self):
# this is only called if we think the timestep has changed; either
# by changing the limits or by editing the resolution
limits = self.limits
if self.auto_res.isChecked():
timestep = round((limits[1]-limits[0])/200.0,5)
else:
timestep = float(self.resolution.text())
self.resolution.setText(str(timestep))
self.timestep = timestep
def region_changed(self, start, end):
# this is the only place where self.limits is set
limits = [ (start - self.start_stamp).to_sec(),
(end - self.start_stamp).to_sec() ]
# cap the limits to the start and end of our bag file
if limits[0]<0:
limits = [0.0,limits[1]]
if limits[1]>(self.end_stamp-self.start_stamp).to_sec():
limits = [limits[0],(self.end_stamp-self.start_stamp).to_sec()]
self.limits = limits
self.recompute_timestep()
self.plot.set_xlim(limits)
self.plot.redraw()
self.update_plot()
def settingsChanged(self):
# resolution changed. recompute the timestep and resample
self.recompute_timestep()
self.update_plot()
def autoChanged(self, state):
if state==2:
# auto mode enabled. recompute the timestep and resample
self.resolution.setDisabled(True)
self.recompute_timestep()
self.update_plot()
else:
# auto mode disabled. enable the resolution text box
# no change to resolution yet, so no need to redraw
self.resolution.setDisabled(False)
def home(self):
# TODO: re-add the button for this. It's useful for restoring the
# X and Y limits so that we can see all of the data
# effectively a "zoom all" button
# reset the plot to our current limits
self.plot.set_xlim(self.limits)
# redraw the plot; this forces a Y autoscaling
self.plot.redraw()
def __init__(self, timeline, parent, topic):
super(PlotWidget, self).__init__(parent)
self.setObjectName('PlotWidget')
self.timeline = timeline
msg_type = self.timeline.get_datatype(topic)
self.msgtopic = topic
self.start_stamp = self.timeline._get_start_stamp()
self.end_stamp = self.timeline._get_end_stamp()
# the current region-of-interest for our bag file
# all resampling and plotting is done with these limits
self.limits = [0, (self.end_stamp - self.start_stamp).to_sec()]
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_bag_plugins'), 'resource',
'plot.ui')
loadUi(ui_file, self)
self.message_tree = MessageTree(msg_type, self)
self.data_tree_layout.addWidget(self.message_tree)
# TODO: make this a dropdown with choices for "Auto", "Full" and
# "Custom"
# I continue to want a "Full" option here
self.auto_res.stateChanged.connect(self.autoChanged)
self.resolution.editingFinished.connect(self.settingsChanged)
self.resolution.setValidator(
QDoubleValidator(0.0, 1000.0, 6, self.resolution))
self.timeline.selected_region_changed.connect(self.region_changed)
self.recompute_timestep()
self.plot = DataPlot(self)
self.plot.set_autoscale(x=False)
self.plot.set_autoscale(y=DataPlot.SCALE_VISIBLE)
self.plot.autoscroll(False)
self.plot.set_xlim(self.limits)
self.data_plot_layout.addWidget(self.plot)
self._home_button = QPushButton()
self._home_button.setToolTip("Reset View")
self._home_button.setIcon(QIcon.fromTheme('go-home'))
self._home_button.clicked.connect(self.home)
self.plot_toolbar_layout.addWidget(self._home_button)
self._config_button = QPushButton("Configure Plot")
self._config_button.clicked.connect(self.plot.doSettingsDialog)
self.plot_toolbar_layout.addWidget(self._config_button)
self.set_cursor(0)
self.paths_on = set()
self._lines = None
# get bag from timeline
bag = None
start_time = self.start_stamp
while bag is None:
bag, entry = self.timeline.get_entry(start_time, topic)
if bag is None:
start_time = self.timeline.get_entry_after(start_time)[1].time
self.bag = bag
# get first message from bag
msg = bag._read_message(entry.position)
self.message_tree.set_message(msg[1])
# state used by threaded resampling
self.resampling_active = False
self.resample_thread = None
self.resample_fields = set()
class PlotWidget(QWidget):
def __init__(self, timeline, parent, topic):
super(PlotWidget, self).__init__(parent)
self.setObjectName('PlotWidget')
self.timeline = timeline
msg_type = self.timeline.get_datatype(topic)
self.msgtopic = topic
self.start_stamp = self.timeline._get_start_stamp()
self.end_stamp = self.timeline._get_end_stamp()
# the current region-of-interest for our bag file
# all resampling and plotting is done with these limits
self.limits = [0, (self.end_stamp - self.start_stamp).to_sec()]
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_bag_plugins'), 'resource',
'plot.ui')
loadUi(ui_file, self)
self.message_tree = MessageTree(msg_type, self)
self.data_tree_layout.addWidget(self.message_tree)
# TODO: make this a dropdown with choices for "Auto", "Full" and
# "Custom"
# I continue to want a "Full" option here
self.auto_res.stateChanged.connect(self.autoChanged)
self.resolution.editingFinished.connect(self.settingsChanged)
self.resolution.setValidator(
QDoubleValidator(0.0, 1000.0, 6, self.resolution))
self.timeline.selected_region_changed.connect(self.region_changed)
self.recompute_timestep()
self.plot = DataPlot(self)
self.plot.set_autoscale(x=False)
self.plot.set_autoscale(y=DataPlot.SCALE_VISIBLE)
self.plot.autoscroll(False)
self.plot.set_xlim(self.limits)
self.data_plot_layout.addWidget(self.plot)
self._home_button = QPushButton()
self._home_button.setToolTip("Reset View")
self._home_button.setIcon(QIcon.fromTheme('go-home'))
self._home_button.clicked.connect(self.home)
self.plot_toolbar_layout.addWidget(self._home_button)
self._config_button = QPushButton("Configure Plot")
self._config_button.clicked.connect(self.plot.doSettingsDialog)
self.plot_toolbar_layout.addWidget(self._config_button)
self.set_cursor(0)
self.paths_on = set()
self._lines = None
# get bag from timeline
bag = None
start_time = self.start_stamp
while bag is None:
bag, entry = self.timeline.get_entry(start_time, topic)
if bag is None:
start_time = self.timeline.get_entry_after(start_time)[1].time
self.bag = bag
# get first message from bag
msg = bag._read_message(entry.position)
self.message_tree.set_message(msg[1])
# state used by threaded resampling
self.resampling_active = False
self.resample_thread = None
self.resample_fields = set()
def set_cursor(self, position):
self.plot.vline(position, color=DataPlot.RED)
self.plot.redraw()
def add_plot(self, path):
self.resample_data([path])
def update_plot(self):
if len(self.paths_on) > 0:
self.resample_data(self.paths_on)
def remove_plot(self, path):
self.plot.remove_curve(path)
self.paths_on.remove(path)
self.plot.redraw()
def load_data(self):
"""get a generator for the specified time range on our bag"""
return self.bag.read_messages(
self.msgtopic,
self.start_stamp + rospy.Duration.from_sec(self.limits[0]),
self.start_stamp + rospy.Duration.from_sec(self.limits[1]))
def resample_data(self, fields):
if self.resample_thread:
# cancel existing thread and join
self.resampling_active = False
self.resample_thread.join()
for f in fields:
self.resample_fields.add(f)
# start resampling thread
self.resampling_active = True
self.resample_thread = threading.Thread(target=self._resample_thread)
# explicitly mark our resampling thread as a daemon, because we don't
# want to block program exit on a long resampling operation
self.resample_thread.setDaemon(True)
self.resample_thread.start()
def _resample_thread(self):
# TODO:
# * look into doing partial display updates for long resampling
# operations
# * add a progress bar for resampling operations
x = {}
y = {}
for path in self.resample_fields:
x[path] = []
y[path] = []
msgdata = self.load_data()
for entry in msgdata:
# detect if we're cancelled and return early
if not self.resampling_active:
return
for path in self.resample_fields:
# this resampling method is very unstable, because it picks
# representative points rather than explicitly representing
# the minimum and maximum values present within a sample
# If the data has spikes, this is particularly bad because they
# will be missed entirely at some resolutions and offsets
if x[path] == [] or (entry[2] - self.start_stamp
).to_sec() - x[path][-1] >= self.timestep:
y_value = entry[1]
for field in path.split('.'):
index = None
if field.endswith(']'):
field = field[:-1]
field, _, index = field.rpartition('[')
y_value = getattr(y_value, field)
if index:
index = int(index)
y_value = y_value[index]
y[path].append(y_value)
x[path].append((entry[2] - self.start_stamp).to_sec())
# TODO: incremental plot updates would go here...
# we should probably do incremental updates based on time;
# that is, push new data to the plot maybe every .5 or .1
# seconds
# time is a more useful metric than, say, messages loaded or
# percentage, because it will give a reasonable refresh rate
# without overloading the computer
# if we had a progress bar, we could emit a signal to update it here
# update the plot with final resampled data
for path in self.resample_fields:
if len(x[path]) < 1:
qWarning("Resampling resulted in 0 data points for %s" % path)
else:
if path in self.paths_on:
self.plot.clear_values(path)
self.plot.update_values(path, x[path], y[path])
else:
self.plot.add_curve(path, path, x[path], y[path])
self.paths_on.add(path)
self.plot.redraw()
self.resample_fields.clear()
self.resampling_active = False
def recompute_timestep(self):
# this is only called if we think the timestep has changed; either
# by changing the limits or by editing the resolution
limits = self.limits
if self.auto_res.isChecked():
timestep = round((limits[1] - limits[0]) / 200.0, 5)
else:
timestep = float(self.resolution.text())
self.resolution.setText(str(timestep))
self.timestep = timestep
def region_changed(self, start, end):
# this is the only place where self.limits is set
limits = [(start - self.start_stamp).to_sec(),
(end - self.start_stamp).to_sec()]
# cap the limits to the start and end of our bag file
if limits[0] < 0:
limits = [0.0, limits[1]]
if limits[1] > (self.end_stamp - self.start_stamp).to_sec():
limits = [limits[0], (self.end_stamp - self.start_stamp).to_sec()]
self.limits = limits
self.recompute_timestep()
self.plot.set_xlim(limits)
self.plot.redraw()
self.update_plot()
def settingsChanged(self):
# resolution changed. recompute the timestep and resample
self.recompute_timestep()
self.update_plot()
def autoChanged(self, state):
if state == 2:
# auto mode enabled. recompute the timestep and resample
self.resolution.setDisabled(True)
self.recompute_timestep()
self.update_plot()
else:
# auto mode disabled. enable the resolution text box
# no change to resolution yet, so no need to redraw
self.resolution.setDisabled(False)
def home(self):
# TODO: re-add the button for this. It's useful for restoring the
# X and Y limits so that we can see all of the data
# effectively a "zoom all" button
# reset the plot to our current limits
self.plot.set_xlim(self.limits)
# redraw the plot; this forces a Y autoscaling
self.plot.redraw()
class BatteryProfileFrame(QFrame):
def __init__(self, parent=None):
super(BatteryProfileFrame, self).__init__(parent)
self._cmd_vel_topic_name = '/mobile_base/commands/velocity'
self._battery_topic_name = "/mobile_base/sensors/core/battery"
self._ui = Ui_battery_profile_frame()
self._motion = None
self.robot_state_subscriber = None
self._motion_thread = None
self._plot_widget = None
def setupUi(self, cmd_vel_topic_name):
self._ui.setupUi(self)
self._cmd_vel_topic_name = cmd_vel_topic_name
self._plot_layout = QVBoxLayout(self._ui.battery_profile_group_box)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self.restore()
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: battery test shutdown")
self._stop()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
self._stop()
self._plot_layout.removeWidget(self._plot_widget)
self._plot_widget = None
def restore(self):
'''
Restore the frame after a hibernate.
'''
self._plot_widget = PlotWidget()
self._plot_widget.setWindowTitle("Battery Profile")
self._plot_widget.topic_edit.setText(self._battery_topic_name)
self._plot_layout.addWidget(self._plot_widget)
self._data_plot = DataPlot(self._plot_widget)
self._data_plot.set_autoscale(y=False)
self._data_plot.set_ylim([0, 180])
self._plot_widget.switch_data_plot_widget(self._data_plot)
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._motion:
self._motion = Rotate(self._cmd_vel_topic_name)
self._motion.init(self._ui.angular_speed_spinbox.value())
if not self.robot_state_subscriber:
self.robot_state_subscriber = rospy.Subscriber(
"/mobile_base/events/robot_state", RobotStateEvent,
self.robot_state_callback)
rospy.sleep(0.5)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._battery_topic_name)
except KeyError:
pass
self._plot_widget.add_topic(self._battery_topic_name)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
'''
Hardcore stoppage - straight to zero.
'''
self._stop()
def _stop(self):
if self._plot_widget:
self._plot_widget.enable_timer(False) # pause plot rendering
if self._motion_thread:
self._motion.stop()
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion.shutdown()
self._motion = None
if self.robot_state_subscriber:
self.robot_state_subscriber.unregister()
self.robot_state_subscriber = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# External Slot Callbacks
##########################################################################
@Slot(str)
def on_cmd_vel_topic_combo_box_currentIndexChanged(self, topic_name):
'''
To be connected to the configuration dock combo box (via the main testsuite frame)
'''
self._cmd_vel_topic_name = topic_name
print("DudetteBattery %s" % topic_name)
##########################################################################
# Ros Callbacks
##########################################################################
def robot_state_callback(self, data):
if data.state == RobotStateEvent.OFFLINE:
self.stop()
