def __init__(self, model, ui_file):
super(NotesWidget, self).__init__()
self._model = model
loadUi(ui_file, self)
self._model.rowsInserted.connect(self.update_status)
self._model.rowsRemoved.connect(self.update_status)
self.table_view.setModel(self._model)
self.table_view.verticalScrollBar().rangeChanged.connect(self._handle_change_scroll)
self._last_scroll_max = self.table_view.verticalScrollBar().maximum()
self.input_field.returnPressed.connect(self._handle_insert_entry)
self.take_button.clicked.connect(self._handle_start_take)
self.load_button.clicked.connect(self._handle_load)
self.save_button.clicked.connect(self._handle_save)
self._delete_shortcut = QShortcut(QKeySequence('Delete'), self)
self._delete_shortcut.activated.connect(self._delete_selected)
self._load_shortcut = QShortcut(QKeySequence('Ctrl+O'), self)
self._load_shortcut.activated.connect(self._handle_load)
self._save_shortcut = QShortcut(QKeySequence('Ctrl+S'), self)
self._save_shortcut.activated.connect(self._handle_save)
def __init__(self, model: QAbstractTableModel, ui_file: str, node: Node):
"""Initialize the NotesWidget.
:param model
:param ui_file Path to ui file.
:param node Handle to a node, used for logging
"""
super(NotesWidget, self).__init__()
self._model = model
self._can_save = True
self._has_autosave = True
self._autosave_file = None
save_path = f"{Path.home()}{self.SAVE_DIRECTORY_FROM_HOME}"
if not os.path.exists(save_path):
os.makedirs(save_path)
self._last_save_file = (
f"{save_path}/note-taker"
f"-{datetime.now().strftime('%Y-%m-%d-%H:%M')}")
self._node = node
loadUi(ui_file, self)
self._model.rowsInserted.connect(lambda parent, first, last: [
self.update_status(),
self._set_saved(False),
self._autosave(first, last, self.INSERT),
])
self._model.rowsRemoved.connect(lambda parent, first, last: [
self.update_status(),
self._set_saved(False),
self._autosave(first, last, self.REMOVE),
])
self.table_view.setModel(self._model)
self.table_view.verticalScrollBar().rangeChanged.connect(
self._handle_change_scroll)
self._last_scroll_max = self.table_view.verticalScrollBar().maximum()
self.input_field.returnPressed.connect(self._handle_insert_entry)
self.take_button.clicked.connect(self._handle_start_take)
self.load_button.clicked.connect(self._handle_load)
self.save_button.clicked.connect(self._handle_save)
self.autosave_check_box.stateChanged.connect(self._handle_autosave)
self.autosave_check_box.setChecked(self._has_autosave)
self.autosave_check_box.setEnabled(False)
self._delete_shortcut = QShortcut(QKeySequence("Delete"), self)
self._delete_shortcut.activated.connect(self._delete_selected)
self._load_shortcut = QShortcut(QKeySequence("Ctrl+O"), self)
self._load_shortcut.activated.connect(self._handle_load)
self._save_shortcut = QShortcut(QKeySequence("Ctrl+S"), self)
self._save_shortcut.activated.connect(self._handle_save)
def __init__(self, node):
super(ExamplesWidget, self).__init__()
self.setObjectName('ExamplesWidget')
self.node = node
pkg_name = 'examples_rqt'
ui_filename = 'examples_rqt.ui'
topic_name = 'cmd_vel'
service_name = 'led_control'
_, package_path = get_resource('packages', pkg_name)
ui_file = os.path.join(package_path, 'share', pkg_name, 'resource', ui_filename)
loadUi(ui_file, self)
self.pub_velocity = Twist()
self.pub_velocity.linear.x = 0.0
self.pub_velocity.angular.z = 0.0
self.sub_velocity = Twist()
self.sub_velocity.linear.x = 0.0
self.sub_velocity.angular.z = 0.0
self.slider_x.setValue(0)
self.lcd_number_x.display(0.0)
self.lcd_number_yaw.display(0.0)
qos_profile = 0
qos = self.get_qos(qos_profile)
self.publisher = self.node.create_publisher(Twist, topic_name, qos)
self.subscriber = self.node.create_subscription(Twist, topic_name, self.cmd_callback, qos)
self.service_server = self.node.create_service(SetBool, service_name, self.srv_callback)
self.service_client = self.node.create_client(SetBool, service_name)
update_timer = QTimer(self)
update_timer.timeout.connect(self.update_data)
update_timer.start(self.redraw_interval)
self.push_button_w.pressed.connect(self.on_increase_x_linear_pressed)
self.push_button_x.pressed.connect(self.on_decrease_x_linear_pressed)
self.push_button_a.pressed.connect(self.on_increase_z_angular_pressed)
self.push_button_d.pressed.connect(self.on_decrease_z_angular_pressed)
self.push_button_s.pressed.connect(self.on_stop_pressed)
self.radio_button_led_on.clicked.connect(self.call_led_service)
self.radio_button_led_off.clicked.connect(self.call_led_service)
self.push_button_w.setShortcut('w')
self.push_button_x.setShortcut('x')
self.push_button_a.setShortcut('a')
self.push_button_d.setShortcut('d')
self.push_button_s.setShortcut('s')
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self.push_button_s.pressed)
self.radio_button_led_on.setShortcut('o')
self.radio_button_led_off.setShortcut('f')
def action_connect(self):
'''
Connects the actions in the top bar to the corresponding functions, and sets their shortcuts
:return:
'''
self._widget.actionNew.triggered.connect(self.new)
self._widget.actionOpen.triggered.connect(self.open)
self._widget.actionSave.triggered.connect(self.save)
self._widget.actionSave_as.triggered.connect(self.save_as)
self._widget.actionInit.triggered.connect(self.goto_init)
self._widget.actionCurrent_Frame.triggered.connect(self.goto_frame)
self._widget.actionNext_Frame.triggered.connect(self.goto_next)
self._widget.actionAnimation.triggered.connect(self.play)
self._widget.actionAnimation_until_Frame.triggered.connect(self.play_until)
self._widget.actionDuplicate_Frame.triggered.connect(self.duplicate)
self._widget.actionDelete_Frame.triggered.connect(self.delete)
self._widget.actionLeft.triggered.connect(lambda: self.mirrorFrame("L"))
self._widget.actionRight.triggered.connect(lambda: self.mirrorFrame("R"))
self._widget.actionInvert.triggered.connect(self.invertFrame)
self._widget.actionUndo.triggered.connect(self.undo)
self._widget.actionRedo.triggered.connect(self.redo)
self._widget.actionHelp.triggered.connect(self.help)
self._widget.buttonRecord.clicked.connect(self.record)
self._widget.buttonRecord.shortcut = QShortcut(QKeySequence("Space"), self._widget)
self._widget.buttonRecord.shortcut.activated.connect(self.record)
self._widget.frameList.keyPressed.connect(self.delete)
self._widget.treeModeSelector.currentIndexChanged.connect(self.treeModeChanged)
def __init__(self, context):
super(ConversationViewPlugin, self).__init__(context)
self.setObjectName('ConversationViewPlugin')
self._widget = QWidget()
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
ui_file = os.path.join(
rospkg.RosPack().get_path('rqt_conversation_view'), 'resource',
'conversation_view.ui')
loadUi(ui_file, self._widget)
context.add_widget(self._widget)
self.send_shortcut = QShortcut(QKeySequence("Ctrl+Return"),
self._widget, self.handle_button_send)
self._widget.buttonSend.clicked.connect(self.handle_button_send)
self._widget.textInput.setFocus()
self.signalAddItem.connect(self.add_item_to_conversation_view)
self.pub_input = rospy.Publisher('recognized_word',
RecognizedWord,
queue_size=10)
rospy.Subscriber('raising_events', RaisingEvents,
self.handle_raising_events)
rospy.Subscriber('reply', Reply, self.handle_reply)
def reset(self):
# The plugin should not call init_node as this is performed by rqt_gui_py.
# Due to restrictions in Qt, you cannot manipulate Qt widgets directly within ROS callbacks,
# because they are running in a different thread.
self.initInteractiveServer(SkirosWidget.widget_id)
self._wmi = wmi.WorldModelInterface(SkirosWidget.widget_id)
self._sli = sli.SkillLayerInterface(SkirosWidget.widget_id)
# Setup a timer to keep interface updated
self.refresh_timer = QTimer()
self.refresh_timer.timeout.connect(self.refresh_timer_cb)
self.refresh_timer.start(500)
self.robot_sub = rospy.Subscriber('/robot_output', String, self.robot_output_cb)
self.robot_text = ""
# World model tab
self._wmi.set_monitor_cb(lambda d: self.wm_update_signal.emit(d))
self._sli.set_monitor_cb(lambda d: self.task_progress_signal.emit(d))
self._snapshot_id = ""
self._snapshot_stamp = rospy.Time.now()
self._wm_mutex = Lock()
self._task_mutex = Lock()
# Skill tab
self.last_executed_skill = ""
self.skill_stop_button.setEnabled(False)
self.space_shortcut = QShortcut(QtGui.QKeySequence(Qt.Key_Space), self)
self.space_shortcut.activated.connect(self.skill_start_stop)
# self.space_shortcut.setContext(QtCore.Qt.WidgetWithChildrenShortcut)
# Log tab
self.log_file = None
self.icons = dict()
def __init__(self, model, ui_file):
super(NotesWidget, self).__init__()
self._model = model
self._can_save = True
self._has_autosave = True
self._autosave_file = None
self._last_save_file = None
loadUi(ui_file, self)
self._model.rowsInserted.connect(lambda parent, first, last: [
self.update_status(),
self._set_saved(False),
self._autosave(first, last, self.INSERT)
])
self._model.rowsRemoved.connect(lambda parent, first, last: [
self.update_status(),
self._set_saved(False),
self._autosave(first, last, self.REMOVE)
])
self.table_view.setModel(self._model)
self.table_view.verticalScrollBar().rangeChanged.connect(
self._handle_change_scroll)
self._last_scroll_max = self.table_view.verticalScrollBar().maximum()
self.input_field.returnPressed.connect(self._handle_insert_entry)
self.take_button.clicked.connect(self._handle_start_take)
self.load_button.clicked.connect(self._handle_load)
self.save_button.clicked.connect(self._handle_save)
self.autosave_check_box.stateChanged.connect(self._handle_autosave)
self.autosave_check_box.setChecked(self._has_autosave)
self.autosave_check_box.setEnabled(False)
self._delete_shortcut = QShortcut(QKeySequence('Delete'), self)
self._delete_shortcut.activated.connect(self._delete_selected)
self._load_shortcut = QShortcut(QKeySequence('Ctrl+O'), self)
self._load_shortcut.activated.connect(self._handle_load)
self._save_shortcut = QShortcut(QKeySequence('Ctrl+S'), self)
self._save_shortcut.activated.connect(self._handle_save)
def __init__(self, field, fields_4, sideToggle, robStatusTab,
lb_uebersicht, lb_gamestate, lb_gamescore, lb_extratime):
self.lbHalf = lb_gamestate
self.lbExtraTime = lb_extratime
self.lbGameScore = lb_gamescore
self.tabManager = lb_uebersicht
self.robStatusTab = robStatusTab
self.fields_4 = fields_4
self.field = field
self.sideToggle = sideToggle
#self.lbHalf.setText("GamePhase")
#Key-Shortcut for switching tabs, lbGamePhase just because we need to connect it to one element
QShortcut(QKeySequence("Tab"), self.lbHalf, self.shortcut)
QShortcut(QKeySequence("1"), self.lbHalf, lambda: self.setTab1(0))
QShortcut(QKeySequence("2"), self.lbHalf, lambda: self.setTab1(1))
QShortcut(QKeySequence("3"), self.lbHalf, lambda: self.setTab1(2))
QShortcut(QKeySequence("4"), self.lbHalf, lambda: self.setTab1(3))
sideToggle.stateChanged.connect(lambda: self.changeSides())
def add_buttons(self, colors):
vbx = QVBoxLayout()
i = 1
for color in colors:
but = QPushButton('Color {} ({})'.format(color, i))
but.clicked.connect(self.clicked(color))
but_short = QShortcut(QKeySequence("Ctrl+" + str(i)), self)
but_short.activated.connect(self.clicked(color))
i += 1
vbx.addWidget(but)
vbx.addStretch(1)
self.color_buttons.setLayout(vbx)
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 ExamplesWidget(QWidget):
redraw_interval = 30
publish_interval = 10
cmd_vel_x_factor = 1000.0
cmd_vel_yaw_factor = -10.0
def __init__(self, node):
super(ExamplesWidget, self).__init__()
self.setObjectName('ExamplesWidget')
self.node = node
pkg_name = 'examples_rqt'
ui_filename = 'examples_rqt.ui'
topic_name = 'cmd_vel'
service_name = 'led_control'
_, package_path = get_resource('packages', pkg_name)
ui_file = os.path.join(package_path, 'share', pkg_name, 'resource', ui_filename)
loadUi(ui_file, self)
self.pub_velocity = Twist()
self.pub_velocity.linear.x = 0.0
self.pub_velocity.angular.z = 0.0
self.sub_velocity = Twist()
self.sub_velocity.linear.x = 0.0
self.sub_velocity.angular.z = 0.0
self.slider_x.setValue(0)
self.lcd_number_x.display(0.0)
self.lcd_number_yaw.display(0.0)
qos_profile = 0
qos = self.get_qos(qos_profile)
self.publisher = self.node.create_publisher(Twist, topic_name, qos)
self.subscriber = self.node.create_subscription(Twist, topic_name, self.cmd_callback, qos)
self.service_server = self.node.create_service(SetBool, service_name, self.srv_callback)
self.service_client = self.node.create_client(SetBool, service_name)
update_timer = QTimer(self)
update_timer.timeout.connect(self.update_data)
update_timer.start(self.redraw_interval)
self.push_button_w.pressed.connect(self.on_increase_x_linear_pressed)
self.push_button_x.pressed.connect(self.on_decrease_x_linear_pressed)
self.push_button_a.pressed.connect(self.on_increase_z_angular_pressed)
self.push_button_d.pressed.connect(self.on_decrease_z_angular_pressed)
self.push_button_s.pressed.connect(self.on_stop_pressed)
self.radio_button_led_on.clicked.connect(self.call_led_service)
self.radio_button_led_off.clicked.connect(self.call_led_service)
self.push_button_w.setShortcut('w')
self.push_button_x.setShortcut('x')
self.push_button_a.setShortcut('a')
self.push_button_d.setShortcut('d')
self.push_button_s.setShortcut('s')
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self.push_button_s.pressed)
self.radio_button_led_on.setShortcut('o')
self.radio_button_led_off.setShortcut('f')
def get_qos(self, i):
return {
0: QoSProfile(depth=10),
1: qos_profile_sensor_data,
2: qos_profile_parameters,
3: qos_profile_services_default,
4: qos_profile_parameter_events,
5: qos_profile_system_default,
6: qos_profile_action_status_default}[i]
def cmd_callback(self, msg):
self.sub_velocity = msg
def srv_callback(self, request, response):
if request.data:
self.push_button_led_status.setText('ON')
self.push_button_led_status.setStyleSheet('background-color: yellow')
response.success = True
response.message = 'LED ON'
elif not request.data:
self.push_button_led_status.setText('OFF')
self.push_button_led_status.setStyleSheet('background-color: grey')
response.success = True
response.message = 'LED OFF'
else:
response.success = False
return response
def on_increase_x_linear_pressed(self):
self.pub_velocity.linear.x += 0.01
self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z)
def on_decrease_x_linear_pressed(self):
self.pub_velocity.linear.x -= 0.01
self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z)
def on_increase_z_angular_pressed(self):
self.pub_velocity.angular.z += 0.1
self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z)
def on_decrease_z_angular_pressed(self):
self.pub_velocity.angular.z -= 0.1
self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z)
def on_stop_pressed(self):
self.pub_velocity.linear.x = 0.0
self.pub_velocity.angular.z = 0.0
self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z)
def call_led_service(self):
request = SetBool.Request()
if self.radio_button_led_on.isChecked():
request.data = True
elif self.radio_button_led_off.isChecked():
request.data = False
wait_count = 1
while not self.service_client.wait_for_service(timeout_sec=0.5):
if wait_count > 5:
return
self.node.get_logger().error('Service not available #{0}'.format(wait_count))
wait_count += 1
future = self.service_client.call_async(request)
while rclpy.ok():
if future.done():
if future.result() is not None:
response = future.result()
self.node.get_logger().info(
'Result of service call: {0}'.format(response.message))
else:
self.node.get_logger().error('Error calling service')
break
def send_velocity(self, x_linear, z_angular):
if self.publisher is None:
return
twist = Twist()
twist.linear.x = x_linear
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = z_angular
self.publisher.publish(twist)
def update_data(self):
self.slider_x.setValue(self.sub_velocity.linear.x * self.cmd_vel_x_factor)
self.dial_yaw.setValue(self.sub_velocity.angular.z * self.cmd_vel_yaw_factor)
self.lcd_number_x.display(self.sub_velocity.linear.x)
self.lcd_number_yaw.display(self.sub_velocity.angular.z)
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def trigger_configuration(self):
pass
def __init__(self):
super(MyWidget, self).__init__()
self.brd = CvBridge()
self.view = RGB
ui_file = os.path.join(
rospkg.RosPack().get_path('balloon_color_picker'), 'resource',
'ColorPlugin.ui')
rospy.loginfo('uifile {}'.format(ui_file))
# self.log_info('uifile {}'.format(ui_file))
loadUi(ui_file, self)
# Give QObjects reasonable names
self.setObjectName('ColorPluginUi')
self.uav_name = os.environ['UAV_NAME']
self.orig_h = 920
self.orig_w = 1080
self.hist_hsv_orig_h = 180
self.hist_hsv_orig_w = 256
self.hist_lab_orig_h = 256
self.hist_lab_orig_w = 256
self.hist_status = HSV
self.select_status = HIST_SELECTION
self.crop_stat = IMG
self.hist_mask = np.zeros([self.hist_hsv_orig_h, self.hist_hsv_orig_w])
self.hist_mask_lab = np.zeros(
[self.hist_lab_orig_h, self.hist_lab_orig_w])
self.cur_hist_hs = None
self.cur_hist_ab = None
self.selected_count = 0
# ROS services
# #{ ros services
self.sigma_caller = rospy.ServiceProxy('change_sigma', ChangeSigma)
self.sigma_lab_caller = rospy.ServiceProxy('change_sigma_lab',
ChangeSigmaLab)
self.caller = rospy.ServiceProxy('capture', Capture)
self.capture_cropped_srv = rospy.ServiceProxy('capture_cropped',
CaptureCropped)
self.get_count = rospy.ServiceProxy('get_count', GetCount)
self.clear_count = rospy.ServiceProxy('clear_count', ClearCount)
self.get_config = rospy.ServiceProxy('get_config', GetConfig)
self.get_params = rospy.ServiceProxy('get_params', Params)
self.freeze_service = rospy.ServiceProxy('freeze', Freeze)
self.update_service = rospy.ServiceProxy('change_obd', UpdateObd)
self.change_callback = rospy.ServiceProxy('change_callback',
ChangeCallback)
self.capture_hist = rospy.ServiceProxy('capture_hist', CaptureHist)
# #} end of ros services rospy.wait_for_service('capture')
rospy.loginfo(
'waiting for service "get_params" from computation module \n if this is more than 5 secs check for topic remapping'
)
# self.log_info('waiting for service')
rospy.loginfo('uav_name {}'.format(os.environ['UAV_NAME']))
# self.log_info('uav_name {}'.format(os.environ['UAV_NAME']))
rospy.wait_for_service('get_params')
self.config_path, self.save_path, self.circled_param, self.circle_filter_param, self.circle_luv_param, self.object_detect_param, self.save_to_drone = self.set_params(
)
# SUBS
# #{ ros subs
self.balloon_sub = rospy.Subscriber(self.circled_param,
RosImg,
self.img_callback,
queue_size=1)
self.filter_sub = rospy.Subscriber(self.circle_filter_param,
RosImg,
self.filter_callback,
queue_size=1)
self.filter_luv = rospy.Subscriber(self.circle_luv_param,
RosImg,
self.luv_callback,
queue_size=1)
self.obj_det_sb = rospy.Subscriber(self.object_detect_param,
RosImg,
self.obj_det_callback,
queue_size=1)
# self.hsv = message_filters.Subscriber(self.circle_filter_param, RosImg)
# self.luv = message_filters.Subscriber(self.circle_luv_param, RosImg)
# self.ts = message_filters.ApproximateTimeSynchronizer([self.luv, self.hsv], 1, 0.5)
# self.ts.registerCallback(self.both_callback)
# #} end of ros subs
self.colors = self.load_config(self.config_path)
self.add_buttons(self.colors)
# # DEFAULT IMAGE
# img = cv2.imread('/home/mrs/balloon_workspace/src/ros_packages/balloon_color_picker/data/blue.png')
# cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
# h,w,c = img.shape
# q_img = QImage(img.data, w,h,3*w, QImage.Format_RGB888)
# q = QPixmap.fromImage(q_img)
#DIRECTORY
#default
self.directory.setText(self.save_path + "Red.yaml")
self.color_name = "red"
self.save_button.clicked.connect(self.save_config)
#PLOT
self.figure = Figure()
self.figure_luv = Figure()
self.canvas = FigureCanvas(self.figure)
self.canvas_luv = FigureCanvas(self.figure_luv)
self.canvas.setParent(self.inner)
# self.toolbar = NavigationToolbar(self.canvas,self)
# self.toolbar_luv = NavigationToolbar(self.canvas_luv,self)
# self.toolbar.setParent(self.inner)
# self.toolbar_luv.setParent(self.inner_luv)
self.canvas_luv.setParent(self.inner_luv)
self.inner_luv.hide()
self.inner_luv_hist.hide()
#SLIDER CONFIG
# #{ slider config
self.sigma_slider.setRange(0, 100)
self.sigma_slider.setSingleStep(1)
self.sigma_slider.setValue(6)
self.sigma_slider.valueChanged.connect(self.slider_event)
self.sigma_slider_s.setRange(0, 100)
self.sigma_slider_s.setSingleStep(1)
self.sigma_slider_s.setValue(6)
self.sigma_slider_s.valueChanged.connect(self.slider_event)
self.sigma_slider_v.setRange(0, 100)
self.sigma_slider_v.setSingleStep(1)
self.sigma_slider_v.setValue(80)
self.sigma_slider_v.valueChanged.connect(self.slider_event)
self.sigma_slider_lab.setRange(0, 100)
self.sigma_slider_lab.setSingleStep(1)
self.sigma_slider_lab.setValue(80)
self.sigma_slider_lab.valueChanged.connect(self.slider_event_lab)
self.sigma_slider_a.setRange(0, 100)
self.sigma_slider_a.setSingleStep(1)
self.sigma_slider_a.setValue(6)
self.sigma_slider_a.valueChanged.connect(self.slider_event_lab)
self.sigma_slider_b.setRange(0, 100)
self.sigma_slider_b.setSingleStep(1)
self.sigma_slider_b.setValue(6)
self.sigma_slider_b.valueChanged.connect(self.slider_event_lab)
# #} end of slider config
# #{ font configs
#SIGMA TEXT
font = self.font()
font.setPointSize(16)
self.sigma_value.setFont(font)
self.sigma_value_s.setFont(font)
self.sigma_value_v.setFont(font)
self.sigma_value_lab.setFont(font)
self.sigma_value_a.setFont(font)
self.sigma_value_b.setFont(font)
#IMAGE COUNT TEXT
self.image_count.setFont(font)
#BOX FOR BUTTONS font
# self.color_buttons.setFont(font)
font.setPointSize(14)
self.sigma_value.setFont(font)
self.log_text.setFont(font)
#LAB HSV TEXT
font.setPointSize(23)
self.label_lab.setFont(font)
self.label_lab.hide()
self.label_hsv.setFont(font)
self.label_hsv.hide()
# #} end of font configs
# BUTTONS
self.change.clicked.connect(self.switch_view_hsv)
self.change_both.clicked.connect(self.switch_view_both)
self.change_luv.clicked.connect(self.switch_view_luv)
self.change_object.clicked.connect(self.switch_view_object_detect)
self.capture_button.clicked.connect(self.capture)
self.clear_button.clicked.connect(self.clear)
self.freeze_button.clicked.connect(self.freeze)
self.update_button.clicked.connect(self.update_obd)
# self.wdg_img.setPixmap(q)
# self.box_layout.addWidget(self.toolbar)
# self.inner.box_layout.addWidget(self.canvas)
#shortcuts
self.short_capture = QShortcut(QKeySequence("C"), self)
self.short_capture.activated.connect(self.capture)
self.short_hsv = QShortcut(QKeySequence("1"), self)
self.short_hsv.activated.connect(self.switch_view_hsv)
self.short_lab = QShortcut(QKeySequence("2"), self)
self.short_lab.activated.connect(self.switch_view_luv)
self.short_object_detect = QShortcut(QKeySequence("3"), self)
self.short_object_detect.activated.connect(
self.switch_view_object_detect)
self.short_object_detect_update = QShortcut(QKeySequence("U"), self)
self.short_object_detect_update.activated.connect(self.update_obd)
self.short_both = QShortcut(QKeySequence("4"), self)
self.short_both.activated.connect(self.switch_view_both)
self.short_save = QShortcut(QKeySequence("S"), self)
self.short_save.activated.connect(self.save_config)
self.short_clear = QShortcut(QKeySequence("N"), self)
self.short_clear.activated.connect(self.clear)
self.short_freeze = QShortcut(QKeySequence("F"), self)
self.short_freeze.activated.connect(self.freeze)
vbx = QVBoxLayout()
but_hsv = QRadioButton()
but_hsv.setText('HSV')
but_hsv.setChecked(True)
self.color_space = 'HSV'
but_hsv.clicked.connect(self.set_colorspace_hsv)
vbx.addWidget(but_hsv)
but_lab = QRadioButton()
but_lab.setText('LAB')
but_lab.clicked.connect(self.set_colorspace_lab)
vbx.addWidget(but_lab)
vbx.addStretch(1)
self.radio_buttons.setLayout(vbx)
vbx_method = QVBoxLayout()
but_lut = QRadioButton()
but_lut.setText('LUT')
but_lut.setChecked(False)
but_lut.clicked.connect(self.set_method_lut)
vbx_method.addWidget(but_lut)
but_thr = QRadioButton()
but_thr.setText('Threshold')
but_thr.setChecked(True)
but_thr.clicked.connect(self.set_method_thr)
vbx_method.addWidget(but_thr)
vbx.addStretch(1)
self.radio_buttons_method.setLayout(vbx_method)
self.load_method = 'THR'
# self.mousePressEvent.connect(self.mousePressEvent)
self.plotted = False
self._rubber = None
self.frozen = False
def initialize_shortcuts(self):
self.ctrl_z = QShortcut(QKeySequence(self.tr("Ctrl+Z")), self)
self.ctrl_shift_z = QShortcut(QKeySequence(self.tr("Ctrl+Shift+Z")),
self)
def __init__(self, context):
super(EUFSRobotSteeringGUI, self).__init__(context)
self.setObjectName('EUFSRobotSteeringGUI')
rp = rospkg.RosPack()
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
ui_file = os.path.join(rp.get_path('ros_can_sim'), 'resource',
'EUFSRobotSteeringGUI.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
self._widget.setObjectName('EUFSRobotSteeringGUI')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._publisher = None
self.inputs = ["Speed", "Acceleration", "Jerk"]
self._widget.input_select_menu.addItems(self.inputs)
self._widget.input_select_menu.setCurrentIndex(1)
self._widget.topic_line_edit.textChanged.connect(
self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.linear_slider.valueChanged.connect(
self._on_linear_slider_changed)
self._widget.angular_slider.valueChanged.connect(
self._on_angular_slider_changed)
self._widget.increase_linear_push_button.pressed.connect(
self._on_strong_increase_linear_pressed)
self._widget.reset_linear_push_button.pressed.connect(
self._on_reset_linear_pressed)
self._widget.decrease_linear_push_button.pressed.connect(
self._on_strong_decrease_linear_pressed)
self._widget.increase_angular_push_button.pressed.connect(
self._on_strong_increase_angular_pressed)
self._widget.reset_angular_push_button.pressed.connect(
self._on_reset_angular_pressed)
self._widget.decrease_angular_push_button.pressed.connect(
self._on_strong_decrease_angular_pressed)
self._widget.max_linear_double_spin_box.valueChanged.connect(
self._on_max_linear_changed)
self._widget.min_linear_double_spin_box.valueChanged.connect(
self._on_min_linear_changed)
self._widget.max_angular_double_spin_box.valueChanged.connect(
self._on_max_angular_changed)
self._widget.min_angular_double_spin_box.valueChanged.connect(
self._on_min_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_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_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_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_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_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_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_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_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_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_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_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_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_linear_push_button.setToolTip(
self._widget.increase_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] W)'))
self._widget.reset_linear_push_button.setToolTip(
self._widget.reset_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] X)'))
self._widget.decrease_linear_push_button.setToolTip(
self._widget.decrease_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] S)'))
self._widget.increase_angular_push_button.setToolTip(
self._widget.increase_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] A)'))
self._widget.reset_angular_push_button.setToolTip(
self._widget.reset_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] Z)'))
self._widget.decrease_angular_push_button.setToolTip(
self._widget.decrease_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] D)'))
# timer to consecutively send AckermannDriveStamped 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 ExamplesWidget(QWidget):
def __init__(self, node):
super(ExamplesWidget, self).__init__()
self.setObjectName('ExamplesWidget')
self.node = node
self.REDRAW_INTERVAL = 30
self.PUBLISH_INTERVAL = 100
self.CMD_VEL_X_FACTOR = 1000.0
self.CMD_VEL_YAW_FACTOR = -10.0
pkg_name = 'rqt_example'
ui_filename = 'rqt_example.ui'
topic_name = 'cmd_vel'
service_name = 'led_control'
_, package_path = get_resource('packages', pkg_name)
ui_file = os.path.join(package_path, 'share', pkg_name, 'resource',
ui_filename)
loadUi(ui_file, self)
self.pub_velocity = Twist()
self.pub_velocity.linear.x = 0.0
self.pub_velocity.angular.z = 0.0
self.sub_velocity = Twist()
self.sub_velocity.linear.x = 0.0
self.sub_velocity.angular.z = 0.0
self.slider_x.setValue(0)
self.lcd_number_x.display(0.0)
self.lcd_number_yaw.display(0.0)
qos = QoSProfile(depth=10)
self.publisher = self.node.create_publisher(Twist, topic_name, qos)
self.subscriber = self.node.create_subscription(
Twist, topic_name, self.get_velocity, qos)
self.service_server = self.node.create_service(SetBool, service_name,
self.set_led_status)
self.service_client = self.node.create_client(SetBool, service_name)
self.publish_timer = QTimer(self)
self.publish_timer.timeout.connect(self.send_velocity)
self.publish_timer.start(self.PUBLISH_INTERVAL)
self.update_timer = QTimer(self)
self.update_timer.timeout.connect(self.update_indicators)
self.update_timer.start(self.REDRAW_INTERVAL)
self.push_button_w.pressed.connect(self.increase_linear_x)
self.push_button_x.pressed.connect(self.decrease_linear_x)
self.push_button_a.pressed.connect(self.increase_angular_z)
self.push_button_d.pressed.connect(self.decrease_angular_z)
self.push_button_s.pressed.connect(self.set_stop)
self.push_button_w.setShortcut('w')
self.push_button_x.setShortcut('x')
self.push_button_a.setShortcut('a')
self.push_button_d.setShortcut('d')
self.push_button_s.setShortcut('s')
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self.push_button_s.pressed)
self.radio_button_led_on.clicked.connect(self.call_led_service)
self.radio_button_led_off.clicked.connect(self.call_led_service)
self.radio_button_led_on.setShortcut('o')
self.radio_button_led_off.setShortcut('f')
def get_velocity(self, msg):
self.sub_velocity = msg
def set_led_status(self, request, response):
if request.data:
self.push_button_led_status.setText('ON')
self.push_button_led_status.setStyleSheet(
'color: rgb(255, 170, 0);')
response.success = True
response.message = 'LED ON'
elif not request.data:
self.push_button_led_status.setText('OFF')
self.push_button_led_status.setStyleSheet('')
response.success = True
response.message = 'LED OFF'
else:
response.success = False
return response
def increase_linear_x(self):
self.pub_velocity.linear.x += 0.1
def decrease_linear_x(self):
self.pub_velocity.linear.x -= 0.1
def increase_angular_z(self):
self.pub_velocity.angular.z += 0.1
def decrease_angular_z(self):
self.pub_velocity.angular.z -= 0.1
def set_stop(self):
self.pub_velocity.linear.x = 0.0
self.pub_velocity.angular.z = 0.0
def call_led_service(self):
request = SetBool.Request()
if self.radio_button_led_on.isChecked():
request.data = True
elif self.radio_button_led_off.isChecked():
request.data = False
wait_count = 1
while not self.service_client.wait_for_service(timeout_sec=0.5):
if wait_count > 5:
return
self.node.get_logger().error(
'Service not available #{0}'.format(wait_count))
wait_count += 1
future = self.service_client.call_async(request)
while rclpy.ok():
if future.done():
if future.result() is not None:
response = future.result()
self.node.get_logger().info(
'Result of service call: {0}'.format(response.message))
else:
self.node.get_logger().error('Error calling service')
break
def send_velocity(self):
twist = Twist()
twist.linear.x = self.pub_velocity.linear.x
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = self.pub_velocity.angular.z
self.publisher.publish(twist)
def update_indicators(self):
self.slider_x.setValue(self.sub_velocity.linear.x *
self.CMD_VEL_X_FACTOR)
self.dial_yaw.setValue(self.sub_velocity.angular.z *
self.CMD_VEL_YAW_FACTOR)
self.lcd_number_x.display(self.sub_velocity.linear.x)
self.lcd_number_yaw.display(self.sub_velocity.angular.z)
def shutdown_widget(self):
self.update_timer.stop()
self.publish_timer.stop()
self.node.destroy_client(self.service_client)
self.node.destroy_service(self.service_server)
self.node.destroy_subscription(self.subscriber)
self.node.destroy_publisher(self.publisher)
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()
if topic == '':
return
try:
self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
except TypeError:
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
# 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)
else:
self.zero_cmd_sent = False
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))
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()
try:
self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
except TypeError:
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
# 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)
else:
self.zero_cmd_sent = False
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, node):
super(ExamplesWidget, self).__init__()
self.setObjectName('ExamplesWidget')
self.node = node
self.REDRAW_INTERVAL = 30
self.PUBLISH_INTERVAL = 100
self.CMD_VEL_X_FACTOR = 1000.0
self.CMD_VEL_YAW_FACTOR = -10.0
pkg_name = 'rqt_example'
ui_filename = 'rqt_example.ui'
topic_name = 'cmd_vel'
service_name = 'led_control'
_, package_path = get_resource('packages', pkg_name)
ui_file = os.path.join(package_path, 'share', pkg_name, 'resource',
ui_filename)
loadUi(ui_file, self)
self.pub_velocity = Twist()
self.pub_velocity.linear.x = 0.0
self.pub_velocity.angular.z = 0.0
self.sub_velocity = Twist()
self.sub_velocity.linear.x = 0.0
self.sub_velocity.angular.z = 0.0
self.slider_x.setValue(0)
self.lcd_number_x.display(0.0)
self.lcd_number_yaw.display(0.0)
qos = QoSProfile(depth=10)
self.publisher = self.node.create_publisher(Twist, topic_name, qos)
self.subscriber = self.node.create_subscription(
Twist, topic_name, self.get_velocity, qos)
self.service_server = self.node.create_service(SetBool, service_name,
self.set_led_status)
self.service_client = self.node.create_client(SetBool, service_name)
self.publish_timer = QTimer(self)
self.publish_timer.timeout.connect(self.send_velocity)
self.publish_timer.start(self.PUBLISH_INTERVAL)
self.update_timer = QTimer(self)
self.update_timer.timeout.connect(self.update_indicators)
self.update_timer.start(self.REDRAW_INTERVAL)
self.push_button_w.pressed.connect(self.increase_linear_x)
self.push_button_x.pressed.connect(self.decrease_linear_x)
self.push_button_a.pressed.connect(self.increase_angular_z)
self.push_button_d.pressed.connect(self.decrease_angular_z)
self.push_button_s.pressed.connect(self.set_stop)
self.push_button_w.setShortcut('w')
self.push_button_x.setShortcut('x')
self.push_button_a.setShortcut('a')
self.push_button_d.setShortcut('d')
self.push_button_s.setShortcut('s')
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self.push_button_s.pressed)
self.radio_button_led_on.clicked.connect(self.call_led_service)
self.radio_button_led_off.clicked.connect(self.call_led_service)
self.radio_button_led_on.setShortcut('o')
self.radio_button_led_off.setShortcut('f')
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 EUFSRobotSteeringGUI(Plugin):
slider_factor = 1000.0
def __init__(self, context):
super(EUFSRobotSteeringGUI, self).__init__(context)
self.setObjectName('EUFSRobotSteeringGUI')
rp = rospkg.RosPack()
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
ui_file = os.path.join(rp.get_path('ros_can_sim'), 'resource',
'EUFSRobotSteeringGUI.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
self._widget.setObjectName('EUFSRobotSteeringGUI')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._publisher = None
self.inputs = ["Speed", "Acceleration", "Jerk"]
self._widget.input_select_menu.addItems(self.inputs)
self._widget.input_select_menu.setCurrentIndex(1)
self._widget.topic_line_edit.textChanged.connect(
self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.linear_slider.valueChanged.connect(
self._on_linear_slider_changed)
self._widget.angular_slider.valueChanged.connect(
self._on_angular_slider_changed)
self._widget.increase_linear_push_button.pressed.connect(
self._on_strong_increase_linear_pressed)
self._widget.reset_linear_push_button.pressed.connect(
self._on_reset_linear_pressed)
self._widget.decrease_linear_push_button.pressed.connect(
self._on_strong_decrease_linear_pressed)
self._widget.increase_angular_push_button.pressed.connect(
self._on_strong_increase_angular_pressed)
self._widget.reset_angular_push_button.pressed.connect(
self._on_reset_angular_pressed)
self._widget.decrease_angular_push_button.pressed.connect(
self._on_strong_decrease_angular_pressed)
self._widget.max_linear_double_spin_box.valueChanged.connect(
self._on_max_linear_changed)
self._widget.min_linear_double_spin_box.valueChanged.connect(
self._on_min_linear_changed)
self._widget.max_angular_double_spin_box.valueChanged.connect(
self._on_max_angular_changed)
self._widget.min_angular_double_spin_box.valueChanged.connect(
self._on_min_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_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_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_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_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_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_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_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_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_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_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_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_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_linear_push_button.setToolTip(
self._widget.increase_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] W)'))
self._widget.reset_linear_push_button.setToolTip(
self._widget.reset_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] X)'))
self._widget.decrease_linear_push_button.setToolTip(
self._widget.decrease_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] S)'))
self._widget.increase_angular_push_button.setToolTip(
self._widget.increase_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] A)'))
self._widget.reset_angular_push_button.setToolTip(
self._widget.reset_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] Z)'))
self._widget.decrease_angular_push_button.setToolTip(
self._widget.decrease_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] D)'))
# timer to consecutively send AckermannDriveStamped 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()
if topic == '':
return
try:
self._publisher = rospy.Publisher(topic,
AckermannDriveStamped,
queue_size=10)
except TypeError:
self._publisher = rospy.Publisher(topic, AckermannDriveStamped)
def _on_stop_pressed(self):
# If the current value of sliders is zero directly send stop AckermannDriveStamped msg
if self._widget.linear_slider.value() == 0 and \
self._widget.angular_slider.value() == 0:
self.zero_cmd_sent = False
self._on_parameter_changed()
else:
self._widget.linear_slider.setValue(0)
self._widget.angular_slider.setValue(0)
def _on_linear_slider_changed(self):
self._widget.current_linear_label.setText(
'%0.2f m/s' % (self._widget.linear_slider.value() /
EUFSRobotSteeringGUI.slider_factor))
self._on_parameter_changed()
def _on_angular_slider_changed(self):
self._widget.current_angular_label.setText(
'%0.2f rad/s' % (self._widget.angular_slider.value() /
EUFSRobotSteeringGUI.slider_factor))
self._on_parameter_changed()
def _on_increase_linear_pressed(self):
self._widget.linear_slider.setValue(
self._widget.linear_slider.value() +
self._widget.linear_slider.singleStep())
def _on_reset_linear_pressed(self):
self._widget.linear_slider.setValue(0)
def _on_decrease_linear_pressed(self):
self._widget.linear_slider.setValue(
self._widget.linear_slider.value() -
self._widget.linear_slider.singleStep())
def _on_increase_angular_pressed(self):
self._widget.angular_slider.setValue(
self._widget.angular_slider.value() +
self._widget.angular_slider.singleStep())
def _on_reset_angular_pressed(self):
self._widget.angular_slider.setValue(0)
def _on_decrease_angular_pressed(self):
self._widget.angular_slider.setValue(
self._widget.angular_slider.value() -
self._widget.angular_slider.singleStep())
def _on_max_linear_changed(self, value):
self._widget.linear_slider.setMaximum(
value * EUFSRobotSteeringGUI.slider_factor)
def _on_min_linear_changed(self, value):
self._widget.linear_slider.setMinimum(
value * EUFSRobotSteeringGUI.slider_factor)
def _on_max_angular_changed(self, value):
self._widget.angular_slider.setMaximum(
value * EUFSRobotSteeringGUI.slider_factor)
def _on_min_angular_changed(self, value):
self._widget.angular_slider.setMinimum(
value * EUFSRobotSteeringGUI.slider_factor)
def _on_strong_increase_linear_pressed(self):
self._widget.linear_slider.setValue(
self._widget.linear_slider.value() +
self._widget.linear_slider.pageStep())
def _on_strong_decrease_linear_pressed(self):
self._widget.linear_slider.setValue(
self._widget.linear_slider.value() -
self._widget.linear_slider.pageStep())
def _on_strong_increase_angular_pressed(self):
self._widget.angular_slider.setValue(
self._widget.angular_slider.value() +
self._widget.angular_slider.pageStep())
def _on_strong_decrease_angular_pressed(self):
self._widget.angular_slider.setValue(
self._widget.angular_slider.value() -
self._widget.angular_slider.pageStep())
def _on_parameter_changed(self):
self._send_ackermann_drive_stamped(
self._widget.linear_slider.value() /
EUFSRobotSteeringGUI.slider_factor,
self._widget.angular_slider.value() /
EUFSRobotSteeringGUI.slider_factor)
def _send_ackermann_drive_stamped(self, linear, angular):
if self._publisher is None:
return
drive = AckermannDriveStamped()
drive.header.stamp = rospy.Time.now()
drive.drive.acceleration = 0
drive.drive.speed = 0
drive.drive.jerk = 0
input = self._widget.input_select_menu.currentIndex()
if self.inputs[input] == "Speed":
drive.drive.speed = linear
elif self.inputs[input] == "Acceleration":
drive.drive.acceleration = linear
elif self.inputs[input] == "Jerk":
drive.drive.jerk = linear
drive.drive.steering_angle = angular
drive.drive.steering_angle_velocity = 0
# Only send the zero command once so other devices can take control
if linear == 0 and angular == 0:
if not self.zero_cmd_sent:
self.zero_cmd_sent = True
self._publisher.publish(drive)
else:
self.zero_cmd_sent = False
self._publisher.publish(drive)
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('input', self.input)
instance_settings.set_value('topic',
self._widget.topic_line_edit.text())
instance_settings.set_value(
'vx_max', self._widget.max_linear_double_spin_box.value())
instance_settings.set_value(
'vx_min', self._widget.min_linear_double_spin_box.value())
instance_settings.set_value(
'vw_max', self._widget.max_angular_double_spin_box.value())
instance_settings.set_value(
'vw_min', self._widget.min_angular_double_spin_box.value())
def restore_settings(self, plugin_settings, instance_settings):
value = instance_settings.value('input', 1)
value = rospy.get_param('~default_input', value)
self.input = value
value = instance_settings.value('topic', '/rqt/command')
value = rospy.get_param('~default_topic', value)
self._widget.topic_line_edit.setText(value)
value = self._widget.max_linear_double_spin_box.value()
value = instance_settings.value('vx_max', value)
value = rospy.get_param('~default_vx_max', value)
self._widget.max_linear_double_spin_box.setValue(float(value))
value = self._widget.min_linear_double_spin_box.value()
value = instance_settings.value('vx_min', value)
value = rospy.get_param('~default_vx_min', value)
self._widget.min_linear_double_spin_box.setValue(float(value))
value = self._widget.max_angular_double_spin_box.value()
value = instance_settings.value('vw_max', value)
value = rospy.get_param('~default_vw_max', value)
self._widget.max_angular_double_spin_box.setValue(float(value))
value = self._widget.min_angular_double_spin_box.value()
value = instance_settings.value('vw_min', value)
value = rospy.get_param('~default_vw_min', value)
self._widget.min_angular_double_spin_box.setValue(float(value))
