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()