Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 import os
00011 from python_qt_binding.QtCore import Signal, Slot, pyqtSlot
00012 from python_qt_binding.QtGui import QFrame, QVBoxLayout
00013 import math
00014
00015 import roslib
00016 roslib.load_manifest('kobuki_qtestsuite')
00017 import rospy
00018 from qt_gui_py_common.worker_thread import WorkerThread
00019 from rqt_plot.plot_widget import PlotWidget
00020 from kobuki_testsuite import Rotate
00021 from kobuki_msgs.msg import RobotStateEvent
00022
00023
00024 import detail.common_rc
00025 import detail.text_rc
00026 from detail.battery_profile_frame_ui import Ui_battery_profile_frame
00027 from full_size_data_plot import FullSizeDataPlot
00028
00029
00030
00031
00032
00033 class BatteryProfileFrame(QFrame):
00034 def __init__(self, parent=None):
00035 super(BatteryProfileFrame, self).__init__(parent)
00036 self._cmd_vel_topic_name = '/mobile_base/commands/velocity'
00037 self._battery_topic_name = "/mobile_base/sensors/core/battery"
00038 self._ui = Ui_battery_profile_frame()
00039 self._motion = None
00040 self.robot_state_subscriber = None
00041 self._motion_thread = None
00042
00043 def setupUi(self, cmd_vel_topic_name):
00044 self._ui.setupUi(self)
00045 self._cmd_vel_topic_name = cmd_vel_topic_name
00046 self._plot_layout = QVBoxLayout(self._ui.battery_profile_group_box)
00047 self._plot_widget = PlotWidget()
00048 self._plot_widget.setWindowTitle("Battery Profile")
00049 self._plot_widget.topic_edit.setText(self._battery_topic_name)
00050 self._plot_layout.addWidget(self._plot_widget)
00051 self._plot_widget.switch_data_plot_widget(FullSizeDataPlot(self._plot_widget))
00052 self._ui.start_button.setEnabled(True)
00053 self._ui.stop_button.setEnabled(False)
00054
00055 def shutdown(self):
00056 '''
00057 Used to terminate the plugin
00058 '''
00059 rospy.loginfo("Kobuki TestSuite: battery test shutdown")
00060 self._stop()
00061
00062
00063
00064
00065
00066 def hibernate(self):
00067 '''
00068 This gets called when the frame goes out of focus (tab switch).
00069 Disable everything to avoid running N tabs in parallel when in
00070 reality we are only running one.
00071 '''
00072 self._stop()
00073
00074 def restore(self):
00075 '''
00076 Restore the frame after a hibernate.
00077 '''
00078 pass
00079
00080
00081
00082
00083
00084
00085 def _run_finished(self):
00086 self._ui.start_button.setEnabled(True)
00087 self._ui.stop_button.setEnabled(False)
00088
00089
00090
00091
00092 @Slot()
00093 def on_start_button_clicked(self):
00094 self._ui.start_button.setEnabled(False)
00095 self._ui.stop_button.setEnabled(True)
00096 if not self._motion:
00097 self._motion = Rotate(self._cmd_vel_topic_name)
00098 self._motion.init(self._ui.angular_speed_spinbox.value())
00099 if not self.robot_state_subscriber:
00100 self.robot_state_subscriber = rospy.Subscriber("/mobile_base/events/robot_state", RobotStateEvent, self.robot_state_callback)
00101 rospy.sleep(0.5)
00102 self._plot_widget._start_time = rospy.get_time()
00103 self._plot_widget.enable_timer(True)
00104 try:
00105 self._plot_widget.remove_topic(self._battery_topic_name)
00106 except KeyError:
00107 pass
00108 self._plot_widget.add_topic(self._battery_topic_name)
00109 self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
00110 self._motion_thread.start()
00111
00112 @Slot()
00113 def on_stop_button_clicked(self):
00114 '''
00115 Hardcore stoppage - straight to zero.
00116 '''
00117 self._stop()
00118
00119 def _stop(self):
00120 self._plot_widget.enable_timer(False)
00121 if self._motion_thread:
00122 self._motion.stop()
00123 self._motion_thread.wait()
00124 self._motion_thread = None
00125 if self._motion:
00126 self._motion.shutdown()
00127 self._motion = None
00128 if self.robot_state_subscriber:
00129 self.robot_state_subscriber.unregister()
00130 self.robot_state_subscriber = None
00131 self._ui.start_button.setEnabled(True)
00132 self._ui.stop_button.setEnabled(False)
00133
00134 @pyqtSlot(float)
00135 def on_angular_speed_spinbox_valueChanged(self, value):
00136 if self._motion:
00137 self._motion.init(self._ui.angular_speed_spinbox.value())
00138
00139
00140
00141
00142 @Slot(str)
00143 def on_cmd_vel_topic_combo_box_currentIndexChanged(self, topic_name):
00144 '''
00145 To be connected to the configuration dock combo box (via the main testsuite frame)
00146 '''
00147 self._cmd_vel_topic_name = topic_name
00148 print("DudetteBattery %s" % topic_name)
00149
00150
00151
00152
00153
00154 def robot_state_callback(self, data):
00155 if data.state == RobotStateEvent.OFFLINE:
00156 self.stop()