def __init__(self, masteruri, screen_name, nodename, user=None, parent=None):
'''
Creates the window, connects the signals and init the class.
'''
QWidget.__init__(self, parent)
# load the UI file
screen_dock_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'ui', 'logscreen', 'ScreenWidget.ui')
loadUi(screen_dock_file, self)
self.setObjectName("ScreenWidget")
self.setWindowIcon(nm.settings().icon('crystal_clear_show_io.png'))
# self.setFeatures(QDockWidget.DockWidgetFloatable | QDockWidget.DockWidgetMovable | QDockWidget.DockWidgetClosable)
self.pauseButton.setIcon(nm.settings().icon('sekkyumu_pause.png'))
self._valid = True
self._logpath = ''
self._lock = threading.RLock()
self.finished = False
self.qfile = None
self.thread = None
self._info = ''
self._masteruri = ''
self._nodename = nodename
self._first_fill = True
self._seek_start = -1
self._seek_end = -1
self._pause_read_end = False
self._ssh_output_file = None
self._ssh_error_file = None
self._ssh_input_file = None
self._on_pause = False
self._char_format_end = None
self.logframe.setVisible(False)
self.loglevelButton.toggled.connect(self.on_toggle_loggers)
self.logger_handler = None
# connect to the button signals
self.output.connect(self._on_output)
self.output_prefix.connect(self._on_output_prefix)
self.error_signal.connect(self._on_error)
self.auth_signal.connect(self.on_request_pw)
self.clearCloseButton.clicked.connect(self.clear)
# self.pauseButton.clicked.connect(self.stop)
self.pauseButton.toggled.connect(self.pause)
self.clear_signal.connect(self.clear)
self.loggerFilterInput.textChanged.connect(self.on_logger_filter_changed)
self.textBrowser.verticalScrollBar().valueChanged.connect(self.on_scrollbar_position_changed)
self.textBrowser.verticalScrollBar().rangeChanged.connect(self.on_scrollbar_range_changed)
self.textBrowser.set_reader(self)
self.tf = TerminalFormats()
self.hl = ScreenHighlighter(self.textBrowser.document())
self.searchFrame.setVisible(False)
self.grepFrame.setVisible(False)
self.grepLineEdit.textChanged.connect(self.on_grep_changed)
self._shortcut_search = QShortcut(QKeySequence(self.tr("Ctrl+F", "Activate search")), self)
self._shortcut_search.activated.connect(self.on_search)
self._shortcut_grep = QShortcut(QKeySequence(self.tr("Ctrl+G", "Activate grep")), self)
self._shortcut_grep.activated.connect(self.on_grep)
self.searchLineEdit.editingFinished.connect(self.on_search_prev)
self.searchNextButton.clicked.connect(self.on_search_next)
self.searchPrevButton.clicked.connect(self.on_search_prev)
# self.visibilityChanged.connect(self.stop)
self._connect(masteruri, screen_name, nodename, user)
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
self.shortcut_x.setContext(Qt.ApplicationShortcut)
self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
self.shortcut_z.setContext(Qt.ApplicationShortcut)
self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
self._update_parameter_timer.start(100)
class RobotSteering(Plugin):
slider_factor = 1000.0
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
self.shortcut_x.setContext(Qt.ApplicationShortcut)
self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
self.shortcut_z.setContext(Qt.ApplicationShortcut)
self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
self._update_parameter_timer.start(100)
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self._unregister_publisher()
self._publisher = rospy.Publisher(topic, Twist)
def _on_stop_pressed(self):
self._widget.x_linear_slider.setValue(0)
self._widget.z_angular_slider.setValue(0)
def _on_x_linear_slider_changed(self):
self._widget.current_x_linear_label.setText('%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_z_angular_slider_changed(self):
self._widget.current_z_angular_label.setText('%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
def _on_reset_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(0)
def _on_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
def _on_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
def _on_reset_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(0)
def _on_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
def _on_max_x_linear_changed(self, value):
self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor)
def _on_min_x_linear_changed(self, value):
self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor)
def _on_max_z_angular_changed(self, value):
self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor)
def _on_min_z_angular_changed(self, value):
self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor)
def _on_strong_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
def _on_strong_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
def _on_strong_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
def _on_strong_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
def _on_parameter_changed(self):
self._send_twist(self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor)
def _send_twist(self, x_linear, z_angular):
if self._publisher is None:
return
twist = Twist()
twist.linear.x = x_linear
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = z_angular
#print(twist)
self._publisher.publish(twist)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._update_parameter_timer.stop()
self._unregister_publisher()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic' , self._widget.topic_line_edit.text())
instance_settings.set_value('vx_max', self._widget.max_x_linear_double_spin_box.value())
instance_settings.set_value('vx_min', self._widget.min_x_linear_double_spin_box.value())
instance_settings.set_value('vw_max', self._widget.max_z_angular_double_spin_box.value())
instance_settings.set_value('vw_min', self._widget.min_z_angular_double_spin_box.value())
def restore_settings(self, plugin_settings, instance_settings):
value = instance_settings.value('topic', "/cmd_vel")
value = rospy.get_param("~default_topic", value)
self._widget.topic_line_edit.setText(value)
value = self._widget.max_x_linear_double_spin_box.value()
value = instance_settings.value( 'vx_max', value)
value = rospy.get_param("~default_vx_max", value)
self._widget.max_x_linear_double_spin_box.setValue(float(value))
value = self._widget.min_x_linear_double_spin_box.value()
value = instance_settings.value( 'vx_min', value)
value = rospy.get_param("~default_vx_min", value)
self._widget.min_x_linear_double_spin_box.setValue(float(value))
value = self._widget.max_z_angular_double_spin_box.value()
value = instance_settings.value( 'vw_max', value)
value = rospy.get_param("~default_vw_max", value)
self._widget.max_z_angular_double_spin_box.setValue(float(value))
value = self._widget.min_z_angular_double_spin_box.value()
value = instance_settings.value( 'vw_min', value)
value = rospy.get_param("~default_vw_min", value)
self._widget.min_z_angular_double_spin_box.setValue(float(value))
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(
self._on_topic_changed)
self._widget.x_linear_slider.valueChanged.connect(
self._on_parameter_changed)
self._widget.z_angular_slider.valueChanged.connect(
self._on_parameter_changed)
self._widget.increase_x_linear_push_button.pressed.connect(
self._on_increase_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(
self._on_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(
self._on_increase_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(
self._on_decrease_z_angular_pressed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W),
self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(
self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S),
self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(
self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A),
self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(
self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D),
self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(
self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space),
self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(
self._on_parameter_changed)
self._update_parameter_timer.start(100)
class RobotSteering(Plugin):
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(
self._on_topic_changed)
self._widget.x_linear_slider.valueChanged.connect(
self._on_parameter_changed)
self._widget.z_angular_slider.valueChanged.connect(
self._on_parameter_changed)
self._widget.increase_x_linear_push_button.pressed.connect(
self._on_increase_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(
self._on_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(
self._on_increase_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(
self._on_decrease_z_angular_pressed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W),
self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(
self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S),
self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(
self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A),
self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(
self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D),
self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(
self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space),
self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(
self._on_parameter_changed)
self._update_parameter_timer.start(100)
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self._unregister_publisher()
self._publisher = rospy.Publisher(topic, Twist)
def _on_parameter_changed(self):
self._send_twist(self._widget.x_linear_slider.value() / 1000.0,
self._widget.z_angular_slider.value() / 1000.0)
def _send_twist(self, x_linear, z_angular):
if self._publisher is None:
return
twist = Twist()
twist.linear.x = x_linear
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = z_angular
self._publisher.publish(twist)
def _on_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() +
self._widget.x_linear_slider.singleStep())
def _on_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() -
self._widget.x_linear_slider.singleStep())
def _on_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() +
self._widget.z_angular_slider.singleStep())
def _on_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() -
self._widget.z_angular_slider.singleStep())
def _on_strong_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() +
self._widget.x_linear_slider.pageStep())
def _on_strong_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() -
self._widget.x_linear_slider.pageStep())
def _on_strong_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() +
self._widget.z_angular_slider.pageStep())
def _on_strong_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() -
self._widget.z_angular_slider.pageStep())
def _on_stop_pressed(self):
self._widget.x_linear_slider.setValue(0)
self._widget.z_angular_slider.setValue(0)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._update_parameter_timer.stop()
self._unregister_publisher()
def save_settings(self, plugin_settings, instance_settings):
topic = self._widget.topic_line_edit.text()
instance_settings.set_value('topic', topic)
def restore_settings(self, plugin_settings, instance_settings):
topic = instance_settings.value('topic', '/cmd_vel')
self._widget.topic_line_edit.setText(topic)
def __init__(self, context):
super(RosBehaviourTree, self).__init__(context)
self.setObjectName('RosBehaviourTree')
parser = argparse.ArgumentParser()
RosBehaviourTree.add_arguments(parser, False)
# if the context doesn't have an argv attribute then assume we're running with --no-roscore
if not hasattr(context, 'argv'):
args = sys.argv[1:]
# Can run the viewer with or without live updating. Running without is
# intended for viewing of bags only
self.live_update = False
else:
args = context.argv()
self.live_update = True
parsed_args = parser.parse_args(args)
self.context = context
self.initialized = False
self._current_dotcode = None # dotcode for the tree that is currently displayed
self._viewing_bag = False # true if a bag file is loaded
# True if next or previous buttons are pressed. Reset if the tree being
# viewed is the last one in the list.
self._browsing_timeline = False
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PygraphvizFactory() # PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosBehaviourTreeDotcodeGenerator()
self.current_topic = None
self.behaviour_sub = None
self._tip_message = None # message of the tip of the tree
self._saved_settings_topic = None # topic subscribed to by previous instance
self.visibility_level = py_trees.common.VisibilityLevel.DETAIL
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_py_trees'), 'resource',
'RosBehaviourTree.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosBehaviourTreeUi')
if hasattr(context, 'serial_number') and context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.highlight_connections_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(
QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_bag_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_bag_push_button.pressed.connect(self._load_bag)
self._widget.load_dot_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(
QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(
self._save_image)
for text in visibility.combo_to_py_trees:
self._widget.visibility_level_combo_box.addItem(text)
self._widget.visibility_level_combo_box.setCurrentIndex(
self.visibility_level)
self._widget.visibility_level_combo_box.currentIndexChanged[
'QString'].connect(self._update_visibility_level)
# set up the function that is called whenever the box is resized -
# ensures that the timeline is correctly drawn.
self._widget.resizeEvent = self._resize_event
self._timeline = None
self._timeline_listener = None
# Connect the message changed function of this object to a corresponding
# signal. This signal will be activated whenever the message being
# viewed changes.
self._message_changed.connect(self.message_changed)
self._message_cleared.connect(self.message_cleared)
# Set up combo box for topic selection
# when the refresh_combo signal happens, update the combo topics available
self._refresh_combo.connect(self._update_combo_topics)
# filter events to catch the event which opens the combo box
self._combo_event_filter = RosBehaviourTree.ComboBoxEventFilter(
self._refresh_combo)
self._widget.topic_combo_box.installEventFilter(
self._combo_event_filter)
self._widget.topic_combo_box.activated.connect(self._choose_topic)
self._update_combo_topics()
# Set up navigation buttons
self._widget.previous_tool_button.pressed.connect(self._previous)
self._widget.previous_tool_button.setIcon(
QIcon.fromTheme('go-previous'))
self._widget.next_tool_button.pressed.connect(self._next)
self._widget.next_tool_button.setIcon(QIcon.fromTheme('go-next'))
self._widget.first_tool_button.pressed.connect(self._first)
self._widget.first_tool_button.setIcon(QIcon.fromTheme('go-first'))
self._widget.last_tool_button.pressed.connect(self._last)
self._widget.last_tool_button.setIcon(QIcon.fromTheme('go-last'))
# play, pause and stop buttons
self._widget.play_tool_button.pressed.connect(self._play)
self._widget.play_tool_button.setIcon(
QIcon.fromTheme('media-playback-start'))
self._widget.stop_tool_button.pressed.connect(self._stop)
self._widget.stop_tool_button.setIcon(
QIcon.fromTheme('media-playback-stop'))
# also connect the navigation buttons so that they stop the timer when
# pressed while the tree is playing.
self._widget.first_tool_button.pressed.connect(self._stop)
self._widget.previous_tool_button.pressed.connect(self._stop)
self._widget.last_tool_button.pressed.connect(self._stop)
self._widget.next_tool_button.pressed.connect(self._stop)
# set up shortcuts for navigation (vim)
next_shortcut_vi = QShortcut(QKeySequence("l"), self._widget)
next_shortcut_vi.activated.connect(
self._widget.next_tool_button.pressed)
previous_shortcut_vi = QShortcut(QKeySequence("h"), self._widget)
previous_shortcut_vi.activated.connect(
self._widget.previous_tool_button.pressed)
first_shortcut_vi = QShortcut(QKeySequence("^"), self._widget)
first_shortcut_vi.activated.connect(
self._widget.first_tool_button.pressed)
last_shortcut_vi = QShortcut(QKeySequence("$"), self._widget)
last_shortcut_vi.activated.connect(
self._widget.last_tool_button.pressed)
# shortcuts for emacs
next_shortcut_emacs = QShortcut(QKeySequence("Ctrl+f"), self._widget)
next_shortcut_emacs.activated.connect(
self._widget.next_tool_button.pressed)
previous_shortcut_emacs = QShortcut(QKeySequence("Ctrl+b"),
self._widget)
previous_shortcut_emacs.activated.connect(
self._widget.previous_tool_button.pressed)
first_shortcut_emacs = QShortcut(QKeySequence("Ctrl+a"), self._widget)
first_shortcut_emacs.activated.connect(
self._widget.first_tool_button.pressed)
last_shortcut_emacs = QShortcut(QKeySequence("Ctrl+e"), self._widget)
last_shortcut_emacs.activated.connect(
self._widget.last_tool_button.pressed)
# set up stuff for dotcode cache
self._dotcode_cache_capacity = 50
self._dotcode_cache = {}
# cache is ordered on timestamps from messages, but earliest timestamp
# isn't necessarily the message that was viewed the longest time ago, so
# need to store keys
self._dotcode_cache_keys = []
# set up stuff for scene cache (dotcode cache doesn't seem to make much difference)
self._scene_cache_capacity = 50
self._scene_cache = {}
self._scene_cache_keys = []
# Update the timeline buttons to correspond with a completely
# uninitialised state.
self._set_timeline_buttons(first_snapshot=False,
previous_snapshot=False,
next_snapshot=False,
last_snapshot=False)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
# This is used to store a timer which controls how fast updates happen when the play button is pressed.
self._play_timer = None
# updates the view
self._refresh_view.connect(self._refresh_tree_graph)
self._force_refresh = False
if self.live_update:
context.add_widget(self._widget)
else:
self.initialized = True # this needs to be set for trees to be displayed
context.setCentralWidget(self._widget)
if parsed_args.bag:
self._load_bag(parsed_args.bag)
elif parsed_args.latest_bag:
# if the latest bag is requested, load it from the default directory, or
# the one specified in the args
bag_dir = parsed_args.bag_dir or os.getenv(
'ROS_HOME', os.path.expanduser('~/.ros')) + '/behaviour_trees'
self.open_latest_bag(bag_dir, parsed_args.by_time)
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
self.shortcut_x.setContext(Qt.ApplicationShortcut)
self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
self.shortcut_z.setContext(Qt.ApplicationShortcut)
self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
class RobotSteering(Plugin):
slider_factor = 1000.0
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
self.shortcut_x.setContext(Qt.ApplicationShortcut)
self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
self.shortcut_z.setContext(Qt.ApplicationShortcut)
self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self._unregister_publisher()
self._publisher = rospy.Publisher(topic, Twist)
self._publisher_heat = rospy.Publisher('tempeture', Int16)
self._sub = rospy.Subscriber("tempeture", Int16, self.heat_callback)
def heat_callback(self, data):
self._widget.current_z_angular_label.setText('%0.2f rad/s' % data.data)
self._on_parameter_changed()
def _on_stop_pressed(self):
self._widget.x_linear_slider.setValue(0)
self._widget.z_angular_slider.setValue(0)
def _on_x_linear_slider_changed(self):
self._widget.current_x_linear_label.setText('%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_z_angular_slider_changed(self):
self._widget.current_z_angular_label.setText('%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
def _on_reset_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(0)
def _on_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
def _on_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
def _on_reset_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(0)
def _on_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
def _on_max_x_linear_changed(self, value):
self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor)
def _on_min_x_linear_changed(self, value):
self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor)
def _on_max_z_angular_changed(self, value):
self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor)
def _on_min_z_angular_changed(self, value):
self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor)
def _on_strong_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
def _on_strong_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
def _on_strong_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
def _on_strong_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
def _on_parameter_changed(self):
self._send_twist(self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor)
def _send_twist(self, x_linear, z_angular):
if self._publisher is None:
return
twist = Twist()
twist.linear.x = x_linear
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = z_angular
heat = Int16()
heat.data = x_linear
# Only send the zero command once so other devices can take control
if x_linear == 0 and z_angular == 0:
if not self.zero_cmd_sent:
self.zero_cmd_sent = True
self._publisher.publish(twist)
self._publisher_heat.publish(heat)
else:
self.zero_cmd_sent = False
self._publisher.publish(twist)
self._publisher_heat.publish(heat)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._update_parameter_timer.stop()
self._unregister_publisher()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic' , self._widget.topic_line_edit.text())
instance_settings.set_value('vx_max', self._widget.max_x_linear_double_spin_box.value())
instance_settings.set_value('vx_min', self._widget.min_x_linear_double_spin_box.value())
instance_settings.set_value('vw_max', self._widget.max_z_angular_double_spin_box.value())
instance_settings.set_value('vw_min', self._widget.min_z_angular_double_spin_box.value())
def restore_settings(self, plugin_settings, instance_settings):
value = instance_settings.value('topic', "/cmd_vel")
value = rospy.get_param("~default_topic", value)
self._widget.topic_line_edit.setText(value)
value = self._widget.max_x_linear_double_spin_box.value()
value = instance_settings.value( 'vx_max', value)
value = rospy.get_param("~default_vx_max", value)
self._widget.max_x_linear_double_spin_box.setValue(float(value))
value = self._widget.min_x_linear_double_spin_box.value()
value = instance_settings.value( 'vx_min', value)
value = rospy.get_param("~default_vx_min", value)
self._widget.min_x_linear_double_spin_box.setValue(float(value))
value = self._widget.max_z_angular_double_spin_box.value()
value = instance_settings.value( 'vw_max', value)
value = rospy.get_param("~default_vw_max", value)
self._widget.max_z_angular_double_spin_box.setValue(float(value))
value = self._widget.min_z_angular_double_spin_box.value()
value = instance_settings.value( 'vw_min', value)
value = rospy.get_param("~default_vw_min", value)
self._widget.min_z_angular_double_spin_box.setValue(float(value))
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._publishermap = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._publisher = rospy.Publisher('speedSP', Int16,queue_size=10)
self._publisher = rospy.Publisher('pid', numpy_msg(Floats))
self._publishermap = rospy.Publisher('syscommand',String)
self._sub = rospy.Subscriber("temperature", Int16, self.heat_callback)
self._subco = rospy.Subscriber("co2", Int16, self.co_callback)
self._subbat = rospy.Subscriber("baterry", Int16, self.bat_callback)
self._subqrr = rospy.Subscriber("/qr_detection1/qrdata", String, self.qrr_callback)
self._subqrl = rospy.Subscriber("/qr_detection2/qrdata", String, self.qrl_callback)
self._subqrc = rospy.Subscriber("/qr_detection3/qrdata", String, self.qrc_callback)
# self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
# self._widget.manual_push_button.pressed.connect(self._on_manual_pressed)
# Kp Ki Kd Slide parameter
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.sliKp.valueChanged.connect(self._on_sliKp_changed)
self._widget.sliKi.valueChanged.connect(self._on_sliKi_changed)
self._widget.sliKd.valueChanged.connect(self._on_sliKd_changed)
# Set parameter button
self._widget.btnSetParam.pressed.connect(self._on_SetParam_pressed)
# self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
# self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
#self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
# self._widget.reset_push_button.pressed.connect(self._on_reset_map_pressed)
#self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
#self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
#self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
#self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
#self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
#self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
# self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
#self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
#self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
#self.shortcut_w.setContext(Qt.ApplicationShortcut)
#self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
#self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
##self.shortcut_x.setContext(Qt.ApplicationShortcut)
#self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
#self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
#self.shortcut_s.setContext(Qt.ApplicationShortcut)
#self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
#self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
#self.shortcut_a.setContext(Qt.ApplicationShortcut)
#self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
#self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
#self.shortcut_z.setContext(Qt.ApplicationShortcut)
#self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
#self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
#self.shortcut_d.setContext(Qt.ApplicationShortcut)
#self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
#self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
##self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
#elf.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
#self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
##self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
#self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
#self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
#self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
#self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
#self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
#self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
#self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
#self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
# self.shortcut_space.activated.connect(self._on_manual_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
# self.shortcut_space.activated.connect(self._on_manual_pressed)
# self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
#self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
#self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
#self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
# self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
# self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
# self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
# self._update_parameter_timer = QTimer(self)
# self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
# self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
class RobotSteering(Plugin):
slider_factor = 1000.0
qr_data = "A"
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._publishermap = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._publisher = rospy.Publisher('speedSP', Int16,queue_size=10)
self._publisher = rospy.Publisher('pid', numpy_msg(Floats))
self._publishermap = rospy.Publisher('syscommand',String)
self._sub = rospy.Subscriber("temperature", Int16, self.heat_callback)
self._subco = rospy.Subscriber("co2", Int16, self.co_callback)
self._subbat = rospy.Subscriber("baterry", Int16, self.bat_callback)
self._subqrr = rospy.Subscriber("/qr_detection1/qrdata", String, self.qrr_callback)
self._subqrl = rospy.Subscriber("/qr_detection2/qrdata", String, self.qrl_callback)
self._subqrc = rospy.Subscriber("/qr_detection3/qrdata", String, self.qrc_callback)
# self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
# self._widget.manual_push_button.pressed.connect(self._on_manual_pressed)
# Kp Ki Kd Slide parameter
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.sliKp.valueChanged.connect(self._on_sliKp_changed)
self._widget.sliKi.valueChanged.connect(self._on_sliKi_changed)
self._widget.sliKd.valueChanged.connect(self._on_sliKd_changed)
# Set parameter button
self._widget.btnSetParam.pressed.connect(self._on_SetParam_pressed)
# self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
# self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
#self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
# self._widget.reset_push_button.pressed.connect(self._on_reset_map_pressed)
#self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
#self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
#self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
#self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
#self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
#self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
# self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
#self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
#self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
#self.shortcut_w.setContext(Qt.ApplicationShortcut)
#self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
#self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
##self.shortcut_x.setContext(Qt.ApplicationShortcut)
#self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
#self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
#self.shortcut_s.setContext(Qt.ApplicationShortcut)
#self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
#self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
#self.shortcut_a.setContext(Qt.ApplicationShortcut)
#self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
#self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
#self.shortcut_z.setContext(Qt.ApplicationShortcut)
#self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
#self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
#self.shortcut_d.setContext(Qt.ApplicationShortcut)
#self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
#self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
##self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
#elf.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
#self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
##self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
#self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
#self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
#self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
#self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
#self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
#self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
#self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
#self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
# self.shortcut_space.activated.connect(self._on_manual_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
# self.shortcut_space.activated.connect(self._on_manual_pressed)
# self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
#self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
#self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
#self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
# self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
# self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
# self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
# self._update_parameter_timer = QTimer(self)
# self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
# self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
@Slot(str)
# def _on_topic_changed(self, topic):
# topic = str(topic)
# self._unregister_publisher()
# self._publisher = rospy.Publisher(topic, Twist)
# self._publisher = rospy.Publisher(topic, Int16)
# self._sub = rospy.Subscriber("tempeture", Int16, self.heat_callback)
# self._subco = rospy.Subscriber("co", Int16, self.co_callback)
def qrr_callback(self, data):
# self._widget.label_3.setText('%s' % data.data)
self._widget.textEdit.append('%s' % data.data)
def qrl_callback(self, data):
self._widget.textEdit.append('%s' % data.data)
def qrc_callback(self, data):
self._widget.textEdit.append('%s' % data.data)
def bat_callback(self, data):
self._widget.baterrySlide.setValue(data.data)
self._on_parameter_changed()
def co_callback(self, data):
self._widget.CO2.setText('%d ppm' % data.data)
self._on_parameter_changed()
def heat_callback(self, data):
self._widget.current_z_angular_label.setText('%d C' % data.data)
self._on_parameter_changed()
def _on_manual_pressed(self):
self._widget.x_linear_slider.setValue(0)
self._widget.sliKp.setValue(0)
self._widget.sliKi.setValue(0)
self._widget.sliKd.setValue(0)
def _on_SetParam_pressed(self):
a = numpy.array([self._widget.sliKp.value()/100,self._widget.sliKi.value()/100,self._widget.sliKd.value()/100,self._widget.x_linear_slider.value() ], dtype=numpy.float32)
self._publisher.publish(a)
def _on_x_linear_slider_changed(self):
self._widget.current_x_linear_label.setText('%d rpm' % self._widget.x_linear_slider.value() )
self._on_pid_parameter_changed()
def _on_reset_map_pressed(self):
remap = String()
remap.data = 'reset'
self._publishermap.publish(remap)
def _on_sliKp_changed(self):
self._widget.txtKp.setText('%0.2f' % (self._widget.sliKp.value()/100) )
self._on_pid_parameter_changed()
def _on_sliKi_changed(self):
self._widget.txtKi.setText('%0.2f' % (self._widget.sliKi.value()/100) )
self._on_pid_parameter_changed()
def _on_sliKd_changed(self):
self._widget.txtKd.setText('%0.2f' % (self._widget.sliKd.value()/100) )
self._on_pid_parameter_changed()
# def _on_increase_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
# def _on_reset_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(50)
#def _on_decrease_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
#def _on_increase_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
# def _on_reset_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(0)
# def _on_decrease_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
# def _on_max_x_linear_changed(self, value):
#self._widget.x_linear_slider.setMaximum(value)
#
# def _on_min_x_linear_changed(self, value):
# self._widget.x_linear_slider.setMinimum(value)
# def _on_max_z_angular_changed(self, value):
# self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor)
# def _on_min_z_angular_changed(self, value):
# self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor)
# def _on_strong_increase_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
# def _on_strong_decrease_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
# def _on_strong_increase_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
#def _on_strong_decrease_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
def _on_pid_parameter_changed(self):
self._send_pid_param(self._widget.sliKp.value())
self._send_pid_param(self._widget.sliKi.value())
self._send_pid_param(self._widget.sliKd.value())
self._send_pid_param(self._widget.x_linear_slider.value())
def _send_pid_param(self,pid):
a = numpy.array([self._widget.sliKp.value()/100,self._widget.sliKi.value()/100,self._widget.sliKd.value()/100,self._widget.x_linear_slider.value() ], dtype=numpy.float32)
#self._publisher.publish(a)
def _send_twist(self, x_linear):
# if self._publisher is None:
# return
# twist = Twist()
#twist.linear.x = x_linear
#twist.linear.y = 0
#twist.linear.z = 0
#twist.angular.x = 0
#twist.angular.y = 0
#twist.angular.z = z_angular
speed = Int16()
speed.data = x_linear
# Only send the zero command once so other devices can take control
if x_linear == 0:
if not self.zero_cmd_sent:
self.zero_cmd_sent = True
# self._publisher.publish(twist)
self._publisher.publish(speed)
else:
self.zero_cmd_sent = False
# self._publisher.publish(twist)
self._publisher.publish(speed)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
# self._update_parameter_timer.stop()
self._unregister_publisher()
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._publishersys = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._publisher = rospy.Publisher('speed', Int16,queue_size=10)
self._publisherth = rospy.Publisher('set_heat', Int16,queue_size=10)
self._publishersys = rospy.Publisher('syscommand',String,queue_size=10)
self._publisherauto = rospy.Publisher('autocommand',String,queue_size=10)
self._sub = rospy.Subscriber("temperature", Int16, self.heat_callback)
self._subco = rospy.Subscriber("co2", Int16, self.co_callback)
self._subbat = rospy.Subscriber("autocommand", String, self.auto_callback)
self._subqrr = rospy.Subscriber("/qrdata", String, self.qrr_callback)
self._subspeed = rospy.Subscriber("speedrobot",String,self.speed_callback)
self._submotion = rospy.Subscriber("/image_motion",String,self.motion_callback)
self._subhazard = rospy.Subscriber("/image_hazard",String,self.hazard_callback)
self._substatus = rospy.Subscriber("status_robot",String,self.status_callback)
# self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
# self._widget.manual_push_button.pressed.connect(self._on_manual_pressed)
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.heat_linear_slider.valueChanged.connect(self._on_heat_linear_slider_changed)
# self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
# self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
#self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
self._widget.reset_push_button.pressed.connect(self._on_reset_map_pressed)
self._widget.start_push_button.pressed.connect(self._on_start_pressed)
self._widget.mode_push_button.pressed.connect(self._on_mode_pressed)
self._widget.radio_push_button.pressed.connect(self._on_radio_pressed)
self._widget.comfirm_button.pressed.connect(self._on_confirm_pressed)
self._widget.ignor_button.pressed.connect(self._on_ignor_pressed)
self._widget.sensor_button.pressed.connect(self._on_sensor_pressed)
self._widget.approach_victim_button.pressed.connect(self._on_approach_pressed)
self._widget.check_sensor_button.pressed.connect(self._on_check_sensor_pressed)
#self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
#self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
#self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
#self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
#self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
#self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
# self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
#self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
#self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
#self.shortcut_w.setContext(Qt.ApplicationShortcut)
#self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
#self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
##self.shortcut_x.setContext(Qt.ApplicationShortcut)
#self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
#self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
#self.shortcut_s.setContext(Qt.ApplicationShortcut)
#self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
#self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
#self.shortcut_a.setContext(Qt.ApplicationShortcut)
#self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
#self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
#self.shortcut_z.setContext(Qt.ApplicationShortcut)
#self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
#self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
#self.shortcut_d.setContext(Qt.ApplicationShortcut)
#self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
#self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
##self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
#elf.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
#self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
##self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
#self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
#self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
#self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
#self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
#self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
#self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
#self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
#self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
# self.shortcut_space.activated.connect(self._on_manual_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
# self.shortcut_space.activated.connect(self._on_manual_pressed)
# self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
#self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
#self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
#self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
# self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
# self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
# self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
# self._update_parameter_timer = QTimer(self)
# self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
# self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
class RobotSteering(Plugin):
mode = True
modesensor = True
slider_factor = 1000.0
qr_data = "A"
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._publishersys = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._publisher = rospy.Publisher('speed', Int16,queue_size=10)
self._publisherth = rospy.Publisher('set_heat', Int16,queue_size=10)
self._publishersys = rospy.Publisher('syscommand',String,queue_size=10)
self._publisherauto = rospy.Publisher('autocommand',String,queue_size=10)
self._sub = rospy.Subscriber("temperature", Int16, self.heat_callback)
self._subco = rospy.Subscriber("co2", Int16, self.co_callback)
self._subbat = rospy.Subscriber("autocommand", String, self.auto_callback)
self._subqrr = rospy.Subscriber("/qrdata", String, self.qrr_callback)
self._subspeed = rospy.Subscriber("speedrobot",String,self.speed_callback)
self._submotion = rospy.Subscriber("/image_motion",String,self.motion_callback)
self._subhazard = rospy.Subscriber("/image_hazard",String,self.hazard_callback)
self._substatus = rospy.Subscriber("status_robot",String,self.status_callback)
# self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
# self._widget.manual_push_button.pressed.connect(self._on_manual_pressed)
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.heat_linear_slider.valueChanged.connect(self._on_heat_linear_slider_changed)
# self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
# self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
#self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
self._widget.reset_push_button.pressed.connect(self._on_reset_map_pressed)
self._widget.start_push_button.pressed.connect(self._on_start_pressed)
self._widget.mode_push_button.pressed.connect(self._on_mode_pressed)
self._widget.radio_push_button.pressed.connect(self._on_radio_pressed)
self._widget.comfirm_button.pressed.connect(self._on_confirm_pressed)
self._widget.ignor_button.pressed.connect(self._on_ignor_pressed)
self._widget.sensor_button.pressed.connect(self._on_sensor_pressed)
self._widget.approach_victim_button.pressed.connect(self._on_approach_pressed)
self._widget.check_sensor_button.pressed.connect(self._on_check_sensor_pressed)
#self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
#self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
#self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
#self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
#self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
#self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
# self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
#self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
#self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
#self.shortcut_w.setContext(Qt.ApplicationShortcut)
#self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
#self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
##self.shortcut_x.setContext(Qt.ApplicationShortcut)
#self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
#self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
#self.shortcut_s.setContext(Qt.ApplicationShortcut)
#self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
#self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
#self.shortcut_a.setContext(Qt.ApplicationShortcut)
#self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
#self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
#self.shortcut_z.setContext(Qt.ApplicationShortcut)
#self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
#self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
#self.shortcut_d.setContext(Qt.ApplicationShortcut)
#self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
#self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
##self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
#elf.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
#self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
##self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
#self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
#self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
#self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
#self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
#self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
#self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
#self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
#self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
#self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
# self.shortcut_space.activated.connect(self._on_manual_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
# self.shortcut_space.activated.connect(self._on_manual_pressed)
# self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
#self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
#self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
#self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
# self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
# self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
# self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
# self._update_parameter_timer = QTimer(self)
# self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
# self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
@Slot(str)
# def _on_topic_changed(self, topic):
# topic = str(topic)
# self._unregister_publisher()
# self._publisher = rospy.Publisher(topic, Twist)
# self._publisher = rospy.Publisher(topic, Int16)
# self._sub = rospy.Subscriber("tempeture", Int16, self.heat_callback)
# self._subco = rospy.Subscriber("co", Int16, self.co_callback)
def auto_callback(self, data):
#self._widget.textEdit.append('autocall')
str = String()
if data.data == "detectC":
self._widget.victim_label.setText('Found Hole')
self._widget.current_z_angular_label.setEnabled(True)
self._widget.label.setEnabled(True)
# self._widget.groupBox.setEnabled(True)
self._widget.comfirm_button.setEnabled(True)
self._widget.approach_victim_button.setEnabled(True)
self._widget.ignor_button.setEnabled(True)
str.data = "stop"
self._publishersys.publish(str)
if data.data == "detectT":
self._widget.victim_label.setText('Found Temp')
self._widget.current_z_angular_label.setEnabled(True)
self._widget.label.setEnabled(True)
# self._widget.groupBox.setEnabled(True)
self._widget.comfirm_button.setEnabled(True)
self._widget.approach_victim_button.setEnabled(True)
self._widget.ignor_button.setEnabled(True)
if data.data == "detectQ":
self._widget.victim_label.setText('QR Found')
self._widget.current_z_angular_label.setEnabled(True)
self._widget.label.setEnabled(True)
def speed_callback(self,data):
self._widget.SP_R.setText('%s' % data.data)
def status_callback(self,data):
self._widget.robot_label.setText('%s' % data.data)
def qrr_callback(self, data):
# self._widget.label_3.setText('%s' % data.data)
self._widget.textEdit.append('%s' % data.data)
def motion_callback(self, data):
self._widget.motion.setText('%s' % data.data)
self._widget.motion.setEnabled(True)
def hazard_callback(self, data):
self._widget.hazard.setText('%s' % data.data)
self._widget.hazard.setEnabled(True)
def co_callback(self, data):
self._widget.CO2.setText('%d ppm' % data.data)
self._widget.CO2.setEnabled(True)
self._on_parameter_changed()
def heat_callback(self, data):
self._widget.current_z_angular_label.setText('%d C' % data.data)
self._on_parameter_changed()
def _on_manual_pressed(self):
self._widget.x_linear_slider.setValue(0)
# self._widget.z_angular_slider.setValue(0)
def _on_x_linear_slider_changed(self):
self._widget.current_x_linear_label.setText('%0.2f m/s' % self._widget.x_linear_slider.value() )
self._on_parameter_changed()
def _on_heat_linear_slider_changed(self):
self._widget.current_heat_linear_label.setText('%0.2f C' % self._widget.heat_linear_slider.value() )
self._on_heat_changed()
def _on_approach_pressed(self):
str = String()
str.data = "getposition"
self._publishersys.publish(str)
d = rospy.Duration(0.2,0)
rospy.sleep(d)
str.data = "approach_victim"
self._publishersys.publish(str)
self._widget.victim_label.setText('Approach Victim..')
def _on_left_pressed(self):
str = String()
str.data = "left_path_gen"
self._publishersys.publish(str)
def _on_right_pressed(self):
str = String()
str.data = "right_path_gen"
self._publishersys.publish(str)
def _on_check_sensor_pressed(self):
str = String()
str.data = "check_sensor"
self._publishersys.publish(str)
self._widget.victim_label.setText('Check SS')
self._widget.CO2.setText('0')
self._widget.current_z_angular_label.setText('0')
def _on_reset_map_pressed(self):
str = String()
str.data = "reset"
self._publishersys.publish(str)
def _on_start_pressed(self):
str = String()
str.data = "start_walk"
self._publishersys.publish(str)
def _on_mode_pressed(self):
str = String()
if RobotSteering.mode :
self._widget.mode_push_button.setText('Auto_Mode')
RobotSteering.mode = False
str.data = "teleop_mode"
else :
self._widget.mode_push_button.setText('Teleop_Mode')
RobotSteering.mode = True
str.data = "auto_mode"
self._publishersys.publish(str)
def _on_radio_pressed(self):
str = String()
str.data="radio_mode"
self._publishersys.publish(str)
self._widget.robot_label.setText('RADIO GOGO GOODLUCK')
def _on_confirm_pressed(self):
str = String()
str.data = "plot_map"
self._publishersys.publish(str)
d = rospy.Duration(0.2, 0)
rospy.sleep(d)
str.data = "finish" ############# plottt mappp ##################
self._publishersys.publish(str)
d = rospy.Duration(0.2, 0)
rospy.sleep(d)
str.data = "go"
self._publishersys.publish(str)
self._widget.motion.setEnabled(False)
self._widget.eye_chart.setEnabled(False)
self._widget.hazard.setEnabled(False)
self._widget.CO2.setEnabled(False)
#self._widget.groupBox.setEnabled(False)
self._widget.comfirm_button.setEnabled(True)
self._widget.approach_victim_button.setEnabled(True)
self._widget.ignor_button.setEnabled(True)
self._widget.current_z_angular_label.setEnabled(False)
self._widget.label.setEnabled(False)
self._widget.victim_label.setText('Searching...')
def _on_ignor_pressed(self):
str = String()
str.data = "ignor"
self._publishersys.publish(str)
d = rospy.Duration(0.5,0)
rospy.sleep(d)
str.data = "go"
self._publishersys.publish(str)
#self._widget.groupBox.setEnabled(False)
self._widget.comfirm_button.setEnabled(True)
self._widget.approach_victim_button.setEnabled(True)
self._widget.ignor_button.setEnabled(True)
self._widget.current_z_angular_label.setEnabled(False)
self._widget.label.setEnabled(False)
self._widget.victim_label.setText('Searching...')
def _on_sensor_pressed(self):
str = String()
if RobotSteering.modesensor :
self._widget.sensor_button.setText('Stop_Sensor')
RobotSteering.modesensor = False
str.data = "start_sensor"
else :
self._widget.sensor_button.setText('Start_Sensor')
RobotSteering.modesensor = True
str.data = "stop_sensor"
self._publishersys.publish(str)
# def _on_increase_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
# def _on_reset_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(50)
#def _on_decrease_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
#def _on_increase_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
# def _on_reset_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(0)
# def _on_decrease_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
# def _on_max_x_linear_changed(self, value):
#self._widget.x_linear_slider.setMaximum(value)
#
# def _on_min_x_linear_changed(self, value):
# self._widget.x_linear_slider.setMinimum(value)
# def _on_max_z_angular_changed(self, value):
# self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor)
# def _on_min_z_angular_changed(self, value):
# self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor)
# def _on_strong_increase_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
# def _on_strong_decrease_x_linear_pressed(self):
# self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
# def _on_strong_increase_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
#def _on_strong_decrease_z_angular_pressed(self):
# self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
def _on_parameter_changed(self):
self._send_twist(self._widget.x_linear_slider.value())
def _on_heat_changed(self):
self._send_heat(self._widget.heat_linear_slider.value())
def _send_heat(self, heat):
h = Int16()
h.data = heat
self._publisherth.publish(h)
str = String()
str.data = "setheat"
self._publisherauto.publish(str)
def _send_twist(self, x_linear):
if self._publisher is None:
return
# twist = Twist()
#twist.linear.x = x_linear
#twist.linear.y = 0
#twist.linear.z = 0
#twist.angular.x = 0
#twist.angular.y = 0
#twist.angular.z = z_angular
speed = Int16()
speed.data = x_linear
# Only send the zero command once so other devices can take control
if x_linear == 0:
if not self.zero_cmd_sent:
self.zero_cmd_sent = True
# self._publisher.publish(twist)
self._publisher.publish(speed)
else:
self.zero_cmd_sent = False
# self._publisher.publish(twist)
self._publisher.publish(speed)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
# self._update_parameter_timer.stop()
self._unregister_publisher()