def __init__(self, parent=None):
super(PlotConfiguration, self).__init__(parent)
self._rospack = rospkg.RosPack()
plot_configuration_file = os.path.join(
self._rospack.get_path('sleo_testsuite'), 'resource/ui',
'plot_configuration.ui')
# loadUi(plot_configuration_file, self)
loadUi(plot_configuration_file, self, {'DataPlot': DataPlot})
self._plot_widget = PlotWidget()
self._plot_widget.setWindowTitle("Plot Profile")
self._plot_widget.topic_edit.setText(
"/sleo_velocity_controller/cmd_vel")
self.horizontalLayout_4.addWidget(self._plot_widget)
self._plot_widget.switch_data_plot_widget(self._widget_data_plot)
self._old_cmd_topic = self._cmd_topic_comboBox.currentText()
self._old_odom_topic = self._odom_topic_comboBox.currentText()
# Get current topic list and fileter contains topics we will use
self._topics_and_types = rospy.get_published_topics()
for topic, type in self._topics_and_types:
#print 'topic: '+ topic + ' type: ' + type + '\n'
if type == 'geometry_msgs/Twist':
self._cmd_topic_comboBox.addItem(topic)
if type == 'nav_msgs/Odometry':
self._odom_topic_comboBox.addItem(topic)
def setupUi(self):
self._ui.setupUi(self)
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 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)
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)
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 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 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 setupUi(self):
self._ui.setupUi(self)
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)
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 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 PlotConfiguration(QFrame):
def __init__(self, parent=None):
super(PlotConfiguration, self).__init__(parent)
self._rospack = rospkg.RosPack()
plot_configuration_file = os.path.join(
self._rospack.get_path('sleo_testsuite'), 'resource/ui',
'plot_configuration.ui')
# loadUi(plot_configuration_file, self)
loadUi(plot_configuration_file, self, {'DataPlot': DataPlot})
self._plot_widget = PlotWidget()
self._plot_widget.setWindowTitle("Plot Profile")
self._plot_widget.topic_edit.setText(
"/sleo_velocity_controller/cmd_vel")
self.horizontalLayout_4.addWidget(self._plot_widget)
self._plot_widget.switch_data_plot_widget(self._widget_data_plot)
self._old_cmd_topic = self._cmd_topic_comboBox.currentText()
self._old_odom_topic = self._odom_topic_comboBox.currentText()
# Get current topic list and fileter contains topics we will use
self._topics_and_types = rospy.get_published_topics()
for topic, type in self._topics_and_types:
#print 'topic: '+ topic + ' type: ' + type + '\n'
if type == 'geometry_msgs/Twist':
self._cmd_topic_comboBox.addItem(topic)
if type == 'nav_msgs/Odometry':
self._odom_topic_comboBox.addItem(topic)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Sleo TestSuite: Plot configuration frame shutdown")
def get_cmd_topic(self):
if self._cmd_topic_comboBox.currentText() == '':
rospy.logerr(
"Sleo Testsuite: current command topic is empty! Please checkout whether topic is open or not."
)
return self._cmd_topic_comboBox.currentText()
def get_odom_topic(self):
if self._odom_topic_comboBox.currentText() == '':
rospy.logerr(
"Sleo Testsuite: current odom topic is empty! Please checkout whether topic is open or not."
)
return self._odom_topic_comboBox.currentText()
def run(self, line_error_bool, Line_speed_error_bool, Angl_error_bool,
Angl_speed_error_bool):
self._plot_widget.clean_up_subscribers()
if line_error_bool:
## whether can change to odom_linear_x_topic
odom_linear_x_topic = '/moved_distance_topic'
#self._plot_widget.topic_edit.setText(cmd_linear_x_topic)
#self._plot_widget.topic_edit.setText(odom_speed_linear_x_topic)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(
self._cmd_topic_comboBox.currentText())
except KeyError:
pass
self._plot_widget.add_topic(odom_linear_x_topic)
elif Line_speed_error_bool:
cmd_linear_x_topic = self._cmd_topic_comboBox.currentText(
) + '/linear/x'
## whether can change to odom_linear_x_topic
odom_speed_linear_x_topic = self._odom_topic_comboBox.currentText(
) + '/twist/twist/linear/x'
self._plot_widget.topic_edit.setText(cmd_linear_x_topic)
self._plot_widget.topic_edit.setText(odom_speed_linear_x_topic)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(
self._cmd_topic_comboBox.currentText())
except KeyError:
pass
self._plot_widget.add_topic(cmd_linear_x_topic)
self._plot_widget.add_topic(odom_speed_linear_x_topic)
elif Angl_error_bool:
cmd_angular_z_topic = '/moved_angle_topic'
# self._plot_widget.topic_edit.setText(cmd_angular_z_topic)
# self._plot_widget.topic_edit.setText(odom_speed_linear_x_topic)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(
self._cmd_topic_comboBox.currentText())
except KeyError:
pass
self._plot_widget.add_topic(cmd_angular_z_topic)
elif Angl_speed_error_bool:
cmd_angular_z_topic = self._cmd_topic_comboBox.currentText(
) + '/angular/z'
odom_angular_speed_z_topic = self._odom_topic_comboBox.currentText(
) + '/twist/twist/angular/z'
# self._plot_widget.topic_edit.setText(cmd_angular_z_topic)
# self._plot_widget.topic_edit.setText(odom_speed_linear_x_topic)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(
self._cmd_topic_comboBox.currentText())
except KeyError:
pass
self._plot_widget.add_topic(cmd_angular_z_topic)
self._plot_widget.add_topic(odom_angular_speed_z_topic)
def stop(self):
self._plot_widget.enable_timer(False) # pause plot rendering
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())