class LifeFrame(QFrame):
STATE_STOPPED = "stopped"
STATE_RUN = "running"
STATE_IDLE = "idle"
def __init__(self, parent=None):
super(LifeFrame, self).__init__(parent)
self._ui = Ui_life_frame()
self._motion = Rotate('/mobile_base/commands/velocity')
self._motion_thread = None
self._timer = QTimer()
#self._timer.setInterval(60000) #60s
self._timer.setInterval(250) #60s
QObject.connect(self._timer, SIGNAL('timeout()'), self,
SLOT('update_progress_callback()'))
self._state = LifeFrame.STATE_STOPPED
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: life frame shutdown")
self._motion.shutdown()
##########################################################################
# 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.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def stop(self):
self._state = LifeFrame.STATE_STOPPED
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._timer.start()
self.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
self._timer.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# Timer Callbacks
##########################################################################
@Slot()
def update_progress_callback(self):
if self._state == LifeFrame.STATE_RUN:
new_value = self._ui.run_progress.value() + 1
if new_value == self._ui.run_progress.maximum():
print(" Switching to idle")
self._motion.stop()
self._state = LifeFrame.STATE_IDLE
else:
self._ui.run_progress.setValue(new_value)
if self._state == LifeFrame.STATE_IDLE:
new_value = self._ui.idle_progress.value() + 1
if new_value == self._ui.idle_progress.maximum():
print(" Switching to run")
self.start()
else:
self._ui.idle_progress.setValue(new_value)
class PayloadFrame(QFrame):
def __init__(self, parent=None):
super(PayloadFrame, self).__init__(parent)
self._gyro_topic_name = '/mobile_base/sensors/imu_data'
self._ui = Ui_payload_frame()
self._motion = None
self._motion_thread = None
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: payload frame 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()
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
self._motion_thread = None
self._motion = None
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)
self._motion = Square('/mobile_base/commands/velocity', '/odom', self._gyro_topic_name)
self._motion.init(self._ui.speed_spinbox.value(), self._ui.distance_spinbox.value())
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._stop()
def _stop(self):
if self._motion:
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.speed_spinbox.value(), self._ui.distance_spinbox.value())
@pyqtSlot(float)
def on_distance_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.speed_spinbox.value(), self._ui.distance_spinbox.value())
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
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._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)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
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()
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# 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):
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()
class CliffSensorFrame(QFrame):
STATE_FORWARD = "forward"
STATE_BACKWARD = "backward"
STATE_STOPPED = "stopped"
def __init__(self, parent=None):
super(CliffSensorFrame, self).__init__(parent)
self._ui = Ui_cliff_sensor_frame()
self._motion = TravelForward('/mobile_base/commands/velocity','/odom', '/mobile_base/events/cliff')
self._motion_thread = None
self._distance = 1.2
self._state = CliffSensorFrame.STATE_FORWARD
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.speed_spinbox.value(), self._distance)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: cliff sensor shutdown")
self._motion.shutdown()
##########################################################################
# 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.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
if self._state == CliffSensorFrame.STATE_STOPPED:
return
elif self._state == CliffSensorFrame.STATE_FORWARD:
self._state = CliffSensorFrame.STATE_BACKWARD
self._motion.init(-self._motion.speed, 0.2)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
else:
self._state = CliffSensorFrame.STATE_FORWARD
self._motion.init(-self._motion.speed, self._distance)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._state = CliffSensorFrame.STATE_FORWARD
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._state = CliffSensorFrame.STATE_STOPPED
self.stop()
def stop(self):
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.speed_spinbox.value(), self._distance)
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
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._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._plot_widget.switch_data_plot_widget(FullSizeDataPlot(self._plot_widget))
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
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()
def restore(self):
"""
Restore the frame after a hibernate.
"""
pass
##########################################################################
# 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):
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()
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())
class LifeFrame(QFrame):
STATE_STOPPED = "stopped"
STATE_RUN = "running"
STATE_IDLE = "idle"
def __init__(self, parent=None):
super(LifeFrame, self).__init__(parent)
self._ui = Ui_life_frame()
self._motion = Rotate('/mobile_base/commands/velocity')
self._motion_thread = None
self._timer = QTimer()
#self._timer.setInterval(60000) #60s
self._timer.setInterval(250) #60s
QObject.connect(self._timer, SIGNAL('timeout()'), self, SLOT('update_progress_callback()'))
self._state = LifeFrame.STATE_STOPPED
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: life frame shutdown")
self._motion.shutdown()
##########################################################################
# 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.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def stop(self):
self._state = LifeFrame.STATE_STOPPED
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._timer.start()
self.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
self._timer.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# Timer Callbacks
##########################################################################
@Slot()
def update_progress_callback(self):
if self._state == LifeFrame.STATE_RUN:
new_value = self._ui.run_progress.value()+1
if new_value == self._ui.run_progress.maximum():
print(" Switching to idle")
self._motion.stop()
self._state = LifeFrame.STATE_IDLE
else:
self._ui.run_progress.setValue(new_value)
if self._state == LifeFrame.STATE_IDLE:
new_value = self._ui.idle_progress.value()+1
if new_value == self._ui.idle_progress.maximum():
print(" Switching to run")
self.start()
else:
self._ui.idle_progress.setValue(new_value)
class PayloadFrame(QFrame):
def __init__(self, parent=None):
super(PayloadFrame, self).__init__(parent)
self._gyro_topic_name = '/mobile_base/sensors/imu_data'
self._ui = Ui_payload_frame()
self._motion = None
self._motion_thread = None
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: payload frame 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()
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
self._motion_thread = None
self._motion = None
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)
self._motion = Square('/mobile_base/commands/velocity', '/odom',
self._gyro_topic_name)
self._motion.init(self._ui.speed_spinbox.value(),
self._ui.distance_spinbox.value())
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._stop()
def _stop(self):
if self._motion:
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.speed_spinbox.value(),
self._ui.distance_spinbox.value())
@pyqtSlot(float)
def on_distance_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.speed_spinbox.value(),
self._ui.distance_spinbox.value())
class CliffSensorFrame(QFrame):
STATE_FORWARD = "forward"
STATE_BACKWARD = "backward"
STATE_STOPPED = "stopped"
def __init__(self, parent=None):
super(CliffSensorFrame, self).__init__(parent)
self._ui = Ui_cliff_sensor_frame()
self._motion = TravelForward('/mobile_base/commands/velocity', '/odom',
'/mobile_base/events/cliff')
self._motion_thread = None
self._distance = 1.2
self._state = CliffSensorFrame.STATE_FORWARD
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.speed_spinbox.value(), self._distance)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: cliff sensor shutdown")
self._motion.shutdown()
##########################################################################
# 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.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
if self._state == CliffSensorFrame.STATE_STOPPED:
return
elif self._state == CliffSensorFrame.STATE_FORWARD:
self._state = CliffSensorFrame.STATE_BACKWARD
self._motion.init(-self._motion.speed, 0.2)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
else:
self._state = CliffSensorFrame.STATE_FORWARD
self._motion.init(-self._motion.speed, self._distance)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._state = CliffSensorFrame.STATE_FORWARD
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._state = CliffSensorFrame.STATE_STOPPED
self.stop()
def stop(self):
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.speed_spinbox.value(), self._distance)
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())