def on_start_button_clicked(self): self._ui.start_button.setEnabled(False) self._ui.stop_button.setEnabled(True) if not self._scan_to_angle: self._scan_to_angle = ScanToAngle('/scan',self._laser_scan_angle_topic_name) if not self._motion: self._motion = DriftEstimation(self._laser_scan_angle_topic_name, self._gyro_scan_angle_topic_name, self._error_scan_angle_topic_name, self._cmd_vel_topic_name,self._gyro_topic_name) self._motion.init(self._ui.angular_speed_spinbox.value()) rospy.sleep(0.5) self._plot_widget.data_plot.reset() self._plot_widget._start_time = rospy.get_time() self._plot_widget.enable_timer(True) try: self._plot_widget.remove_topic(self._error_scan_angle_topic_name+'/scan_angle') self._plot_widget_live.remove_topic(self._laser_scan_angle_topic_name+'/scan_angle') self._plot_widget_live.remove_topic(self._gyro_scan_angle_topic_name+'/scan_angle') self._plot_widget_live.remove_topic(self._cmd_vel_topic_name+'/angular/z') except KeyError: pass self._plot_widget_live.add_topic(self._cmd_vel_topic_name+'/angular/z') self._plot_widget_live.add_topic(self._laser_scan_angle_topic_name+'/scan_angle') self._plot_widget_live.add_topic(self._gyro_scan_angle_topic_name+'/scan_angle') self._plot_widget.add_topic(self._error_scan_angle_topic_name+'/scan_angle') self._motion_thread = WorkerThread(self._motion.execute, None) self._motion_thread.start()
def on_start_button_clicked(self): self._ui.start_button.setEnabled(False) self._ui.stop_button.setEnabled(True) if not self._scan_to_angle: self._scan_to_angle = ScanToAngle("/scan", self._laser_scan_angle_topic_name) if not self._motion: self._motion = DriftEstimation( self._laser_scan_angle_topic_name, self._gyro_scan_angle_topic_name, self._error_scan_angle_topic_name, self._cmd_vel_topic_name, self._gyro_topic_name, ) self._motion.init(self._ui.angular_speed_spinbox.value()) rospy.sleep(0.5) self._plot_widget.data_plot.reset() self._plot_widget._start_time = rospy.get_time() self._plot_widget.enable_timer(True) try: self._plot_widget.remove_topic(self._error_scan_angle_topic_name + "/scan_angle") self._plot_widget_live.remove_topic(self._laser_scan_angle_topic_name + "/scan_angle") self._plot_widget_live.remove_topic(self._gyro_scan_angle_topic_name + "/scan_angle") self._plot_widget_live.remove_topic(self._cmd_vel_topic_name + "/angular/z") except KeyError: pass self._plot_widget_live.add_topic(self._cmd_vel_topic_name + "/angular/z") self._plot_widget_live.add_topic(self._laser_scan_angle_topic_name + "/scan_angle") self._plot_widget_live.add_topic(self._gyro_scan_angle_topic_name + "/scan_angle") self._plot_widget.add_topic(self._error_scan_angle_topic_name + "/scan_angle") self._motion_thread = WorkerThread(self._motion.execute, None) self._motion_thread.start()
class GyroDriftFrame(QFrame): def __init__(self, parent=None): super(GyroDriftFrame, self).__init__(parent) self._ui = Ui_gyro_drift_frame() self._laser_scan_angle_topic_name = '/laser_scan_angle' self._gyro_scan_angle_topic_name = '/gyro_scan_angle' self._error_scan_angle_topic_name = '/error_scan_angle' self._cmd_vel_topic_name = '/mobile_base/commands/velocity' self._gyro_topic_name = '/mobile_base/sensors/imu_data' self._motion = None self._scan_to_angle = None self._motion_thread = None self._plot_widget = None self._plot_widget_live = None def setupUi(self): self._ui.setupUi(self) self._plot_layout = QVBoxLayout(self._ui.scan_angle_group_box) self._ui.start_button.setEnabled(True) self._ui.stop_button.setEnabled(False) def shutdown(self): ''' Used to terminate the plugin ''' rospy.loginfo("Kobuki TestSuite: gyro drift shutdown") self._stop() ########################################################################## # Widget Management ########################################################################## def _pause_plots(self): ''' Pause plots, more importantly, pause greedy plot rendering ''' if self._plot_widget: self._plot_widget.enable_timer(False) if self._plot_widget_live: self._plot_widget_live.enable_timer(False) 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_layout.removeWidget(self._plot_widget_live) self._plot_widget = None self._plot_widget_live = None def restore(self): ''' Restore the frame after a hibernate. ''' self._plot_widget = PlotWidget() self._plot_widget.setWindowTitle("Error") self._plot_layout.addWidget(self._plot_widget) self._plot_widget.switch_data_plot_widget(DataPlot(self._plot_widget)) self._plot_widget.data_plot.dynamic_range = True self._plot_widget_live = PlotWidget() self._plot_widget_live.setWindowTitle("Live Graphs") self._plot_widget_live.switch_data_plot_widget(DataPlot(self._plot_widget_live)) self._plot_layout.addWidget(self._plot_widget_live) ########################################################################## # Qt Callbacks ########################################################################## @Slot() def on_start_button_clicked(self): self._ui.start_button.setEnabled(False) self._ui.stop_button.setEnabled(True) if not self._scan_to_angle: self._scan_to_angle = ScanToAngle('/scan',self._laser_scan_angle_topic_name) if not self._motion: self._motion = DriftEstimation(self._laser_scan_angle_topic_name, self._gyro_scan_angle_topic_name, self._error_scan_angle_topic_name, self._cmd_vel_topic_name,self._gyro_topic_name) self._motion.init(self._ui.angular_speed_spinbox.value()) 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._error_scan_angle_topic_name+'/scan_angle') self._plot_widget_live.remove_topic(self._laser_scan_angle_topic_name+'/scan_angle') self._plot_widget_live.remove_topic(self._gyro_scan_angle_topic_name+'/scan_angle') self._plot_widget_live.remove_topic(self._cmd_vel_topic_name+'/angular/z') except KeyError: pass self._plot_widget_live.add_topic(self._cmd_vel_topic_name+'/angular/z') self._plot_widget_live.add_topic(self._laser_scan_angle_topic_name+'/scan_angle') self._plot_widget_live.add_topic(self._gyro_scan_angle_topic_name+'/scan_angle') self._plot_widget.add_topic(self._error_scan_angle_topic_name+'/scan_angle') self._motion_thread = WorkerThread(self._motion.execute, None) self._motion_thread.start() @Slot() def on_stop_button_clicked(self): self._stop() def _stop(self): self._pause_plots() if self._scan_to_angle: self._scan_to_angle.shutdown() self._scan_to_angle = None if self._motion: self._motion.stop() if self._motion_thread: self._motion_thread.wait() self._motion_thread = None if self._motion: self._motion.shutdown() self._motion = 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())
#!/usr/bin/env python # # License: BSD # https://raw.github.com/yujinrobot/kobuki/hydro-devel/kobuki_testsuite/LICENSE # ############################################################################## # Imports ############################################################################## import roslib roslib.load_manifest('kobuki_testsuite') import rospy from kobuki_testsuite import ScanToAngle ############################################################################## # Main ############################################################################## ''' Hold the kinect up in front of a wall. This will calculate the relative heading angle it makes with the wall. ''' if __name__ == '__main__': rospy.init_node('scan_to_angle') s = ScanToAngle('/scan', '/scan_angle') rospy.spin()
class GyroDriftFrame(QFrame): def __init__(self, parent=None): super(GyroDriftFrame, self).__init__(parent) self._ui = Ui_gyro_drift_frame() self._laser_scan_angle_topic_name = "/laser_scan_angle" self._gyro_scan_angle_topic_name = "/gyro_scan_angle" self._error_scan_angle_topic_name = "/error_scan_angle" self._cmd_vel_topic_name = "/mobile_base/commands/velocity" self._gyro_topic_name = "/mobile_base/sensors/imu_data" self._motion = None self._scan_to_angle = None self._motion_thread = None def setupUi(self): self._ui.setupUi(self) self._plot_layout = QVBoxLayout(self._ui.scan_angle_group_box) self._plot_widget = PlotWidget() self._plot_widget.setWindowTitle("Error") self._plot_layout.addWidget(self._plot_widget) self._plot_widget.switch_data_plot_widget(FullSizeDataPlot(self._plot_widget)) self._plot_widget.data_plot.dynamic_range = True self._plot_widget_live = PlotWidget() self._plot_widget_live.setWindowTitle("Live Graphs") self._plot_widget_live.switch_data_plot_widget(MatDataPlot(self._plot_widget_live)) self._plot_layout.addWidget(self._plot_widget_live) self._ui.start_button.setEnabled(True) self._ui.stop_button.setEnabled(False) def shutdown(self): """ Used to terminate the plugin """ rospy.loginfo("Kobuki TestSuite: gyro drift shutdown") self._stop() ########################################################################## # Widget Management ########################################################################## def _pause_plots(self): """ Pause plots, more importantly, pause greedy plot rendering """ self._plot_widget.enable_timer(False) self._plot_widget_live.enable_timer(False) 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() def restore(self): """ Restore the frame after a hibernate. """ pass ########################################################################## # Qt Callbacks ########################################################################## @Slot() def on_start_button_clicked(self): self._ui.start_button.setEnabled(False) self._ui.stop_button.setEnabled(True) if not self._scan_to_angle: self._scan_to_angle = ScanToAngle("/scan", self._laser_scan_angle_topic_name) if not self._motion: self._motion = DriftEstimation( self._laser_scan_angle_topic_name, self._gyro_scan_angle_topic_name, self._error_scan_angle_topic_name, self._cmd_vel_topic_name, self._gyro_topic_name, ) self._motion.init(self._ui.angular_speed_spinbox.value()) rospy.sleep(0.5) self._plot_widget.data_plot.reset() self._plot_widget._start_time = rospy.get_time() self._plot_widget.enable_timer(True) try: self._plot_widget.remove_topic(self._error_scan_angle_topic_name + "/scan_angle") self._plot_widget_live.remove_topic(self._laser_scan_angle_topic_name + "/scan_angle") self._plot_widget_live.remove_topic(self._gyro_scan_angle_topic_name + "/scan_angle") self._plot_widget_live.remove_topic(self._cmd_vel_topic_name + "/angular/z") except KeyError: pass self._plot_widget_live.add_topic(self._cmd_vel_topic_name + "/angular/z") self._plot_widget_live.add_topic(self._laser_scan_angle_topic_name + "/scan_angle") self._plot_widget_live.add_topic(self._gyro_scan_angle_topic_name + "/scan_angle") self._plot_widget.add_topic(self._error_scan_angle_topic_name + "/scan_angle") self._motion_thread = WorkerThread(self._motion.execute, None) self._motion_thread.start() @Slot() def on_stop_button_clicked(self): self._stop() def _stop(self): self._pause_plots() if self._scan_to_angle: self._scan_to_angle.shutdown() self._scan_to_angle = None if self._motion: self._motion.stop() if self._motion_thread: self._motion_thread.wait() self._motion_thread = None if self._motion: self._motion.shutdown() self._motion = 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())