robot_steering.py
Go to the documentation of this file.
1 # Copyright (c) 2011, Dirk Thomas, TU Darmstadt
2 # All rights reserved.
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions
6 # are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above
11 # copyright notice, this list of conditions and the following
12 # disclaimer in the documentation and/or other materials provided
13 # with the distribution.
14 # * Neither the name of the TU Darmstadt nor the names of its
15 # contributors may be used to endorse or promote products derived
16 # from this software without specific prior written permission.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22 # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
28 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 # POSSIBILITY OF SUCH DAMAGE.
30 
31 from __future__ import division
32 import os
33 import rospkg
34 
35 from geometry_msgs.msg import Twist
36 import rospy
37 from python_qt_binding import loadUi
38 from python_qt_binding.QtCore import Qt, QTimer, Slot
39 from python_qt_binding.QtGui import QKeySequence
40 from python_qt_binding.QtWidgets import QShortcut, QWidget
41 from rqt_gui_py.plugin import Plugin
42 
43 
45 
46  slider_factor = 1000.0
47 
48  def __init__(self, context):
49  super(RobotSteering, self).__init__(context)
50  self.setObjectName('RobotSteering')
51 
52  self._publisher = None
53 
54  self._widget = QWidget()
55  rp = rospkg.RosPack()
56  ui_file = os.path.join(
57  rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
58  loadUi(ui_file, self._widget)
59  self._widget.setObjectName('RobotSteeringUi')
60  if context.serial_number() > 1:
61  self._widget.setWindowTitle(
62  self._widget.windowTitle() + (' (%d)' % context.serial_number()))
63  context.add_widget(self._widget)
64 
65  self._widget.topic_line_edit.textChanged.connect(
66  self._on_topic_changed)
67  self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
68 
69  self._widget.x_linear_slider.valueChanged.connect(
71  self._widget.z_angular_slider.valueChanged.connect(
73 
74  self._widget.increase_x_linear_push_button.pressed.connect(
76  self._widget.reset_x_linear_push_button.pressed.connect(
78  self._widget.decrease_x_linear_push_button.pressed.connect(
80  self._widget.increase_z_angular_push_button.pressed.connect(
82  self._widget.reset_z_angular_push_button.pressed.connect(
84  self._widget.decrease_z_angular_push_button.pressed.connect(
86 
87  self._widget.max_x_linear_double_spin_box.valueChanged.connect(
89  self._widget.min_x_linear_double_spin_box.valueChanged.connect(
91  self._widget.max_z_angular_double_spin_box.valueChanged.connect(
93  self._widget.min_z_angular_double_spin_box.valueChanged.connect(
95 
96  self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
97  self.shortcut_w.setContext(Qt.ApplicationShortcut)
98  self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
99  self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
100  self.shortcut_x.setContext(Qt.ApplicationShortcut)
101  self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
102  self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
103  self.shortcut_s.setContext(Qt.ApplicationShortcut)
104  self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
105  self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
106  self.shortcut_a.setContext(Qt.ApplicationShortcut)
107  self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
108  self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
109  self.shortcut_z.setContext(Qt.ApplicationShortcut)
110  self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
111  self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
112  self.shortcut_d.setContext(Qt.ApplicationShortcut)
113  self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
114 
115  self.shortcut_shift_w = QShortcut(
116  QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
117  self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
118  self.shortcut_shift_w.activated.connect(
120  self.shortcut_shift_x = QShortcut(
121  QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
122  self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
123  self.shortcut_shift_x.activated.connect(
125  self.shortcut_shift_s = QShortcut(
126  QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
127  self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
128  self.shortcut_shift_s.activated.connect(
130  self.shortcut_shift_a = QShortcut(
131  QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
132  self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
133  self.shortcut_shift_a.activated.connect(
135  self.shortcut_shift_z = QShortcut(
136  QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
137  self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
138  self.shortcut_shift_z.activated.connect(
140  self.shortcut_shift_d = QShortcut(
141  QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
142  self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
143  self.shortcut_shift_d.activated.connect(
145 
146  self.shortcut_space = QShortcut(
147  QKeySequence(Qt.Key_Space), self._widget)
148  self.shortcut_space.setContext(Qt.ApplicationShortcut)
149  self.shortcut_space.activated.connect(self._on_stop_pressed)
150  self.shortcut_space = QShortcut(
151  QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
152  self.shortcut_space.setContext(Qt.ApplicationShortcut)
153  self.shortcut_space.activated.connect(self._on_stop_pressed)
154 
155  self._widget.stop_push_button.setToolTip(
156  self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
157  self._widget.increase_x_linear_push_button.setToolTip(
158  self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
159  self._widget.reset_x_linear_push_button.setToolTip(
160  self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
161  self._widget.decrease_x_linear_push_button.setToolTip(
162  self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
163  self._widget.increase_z_angular_push_button.setToolTip(
164  self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
165  self._widget.reset_z_angular_push_button.setToolTip(
166  self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
167  self._widget.decrease_z_angular_push_button.setToolTip(
168  self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
169 
170  # timer to consecutively send twist messages
171  self._update_parameter_timer = QTimer(self)
172  self._update_parameter_timer.timeout.connect(
174  self._update_parameter_timer.start(100)
175  self.zero_cmd_sent = False
176 
177  @Slot(str)
178  def _on_topic_changed(self, topic):
179  topic = str(topic)
180  self._unregister_publisher()
181  if topic == '':
182  return
183  try:
184  self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
185  except TypeError:
186  self._publisher = rospy.Publisher(topic, Twist)
187 
188  def _on_stop_pressed(self):
189  # If the current value of sliders is zero directly send stop twist msg
190  if self._widget.x_linear_slider.value() == 0 and \
191  self._widget.z_angular_slider.value() == 0:
192  self.zero_cmd_sent = False
193  self._on_parameter_changed()
194  else:
195  self._widget.x_linear_slider.setValue(0)
196  self._widget.z_angular_slider.setValue(0)
197 
199  self._widget.current_x_linear_label.setText(
200  '%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor))
201  self._on_parameter_changed()
202 
204  self._widget.current_z_angular_label.setText(
205  '%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor))
206  self._on_parameter_changed()
207 
209  self._widget.x_linear_slider.setValue(
210  self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
211 
213  self._widget.x_linear_slider.setValue(0)
214 
216  self._widget.x_linear_slider.setValue(
217  self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
218 
220  self._widget.z_angular_slider.setValue(
221  self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
222 
224  self._widget.z_angular_slider.setValue(0)
225 
227  self._widget.z_angular_slider.setValue(
228  self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
229 
230  def _on_max_x_linear_changed(self, value):
231  self._widget.x_linear_slider.setMaximum(
232  value * RobotSteering.slider_factor)
233 
234  def _on_min_x_linear_changed(self, value):
235  self._widget.x_linear_slider.setMinimum(
236  value * RobotSteering.slider_factor)
237 
238  def _on_max_z_angular_changed(self, value):
239  self._widget.z_angular_slider.setMaximum(
240  value * RobotSteering.slider_factor)
241 
242  def _on_min_z_angular_changed(self, value):
243  self._widget.z_angular_slider.setMinimum(
244  value * RobotSteering.slider_factor)
245 
247  self._widget.x_linear_slider.setValue(
248  self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
249 
251  self._widget.x_linear_slider.setValue(
252  self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
253 
255  self._widget.z_angular_slider.setValue(
256  self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
257 
259  self._widget.z_angular_slider.setValue(
260  self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
261 
263  self._send_twist(
264  self._widget.x_linear_slider.value() / RobotSteering.slider_factor,
265  self._widget.z_angular_slider.value() / RobotSteering.slider_factor)
266 
267  def _send_twist(self, x_linear, z_angular):
268  if self._publisher is None:
269  return
270  twist = Twist()
271  twist.linear.x = x_linear
272  twist.linear.y = 0
273  twist.linear.z = 0
274  twist.angular.x = 0
275  twist.angular.y = 0
276  twist.angular.z = z_angular
277 
278  # Only send the zero command once so other devices can take control
279  if x_linear == 0 and z_angular == 0:
280  if not self.zero_cmd_sent:
281  self.zero_cmd_sent = True
282  self._publisher.publish(twist)
283  else:
284  self.zero_cmd_sent = False
285  self._publisher.publish(twist)
286 
288  if self._publisher is not None:
289  self._publisher.unregister()
290  self._publisher = None
291 
292  def shutdown_plugin(self):
293  self._update_parameter_timer.stop()
294  self._unregister_publisher()
295 
296  def save_settings(self, plugin_settings, instance_settings):
297  instance_settings.set_value(
298  'topic', self._widget.topic_line_edit.text())
299  instance_settings.set_value(
300  'vx_max', self._widget.max_x_linear_double_spin_box.value())
301  instance_settings.set_value(
302  'vx_min', self._widget.min_x_linear_double_spin_box.value())
303  instance_settings.set_value(
304  'vw_max', self._widget.max_z_angular_double_spin_box.value())
305  instance_settings.set_value(
306  'vw_min', self._widget.min_z_angular_double_spin_box.value())
307 
308  def restore_settings(self, plugin_settings, instance_settings):
309  value = instance_settings.value('topic', '/cmd_vel')
310  value = rospy.get_param('~default_topic', value)
311  self._widget.topic_line_edit.setText(value)
312 
313  value = self._widget.max_x_linear_double_spin_box.value()
314  value = instance_settings.value('vx_max', value)
315  value = rospy.get_param('~default_vx_max', value)
316  self._widget.max_x_linear_double_spin_box.setValue(float(value))
317 
318  value = self._widget.min_x_linear_double_spin_box.value()
319  value = instance_settings.value('vx_min', value)
320  value = rospy.get_param('~default_vx_min', value)
321  self._widget.min_x_linear_double_spin_box.setValue(float(value))
322 
323  value = self._widget.max_z_angular_double_spin_box.value()
324  value = instance_settings.value('vw_max', value)
325  value = rospy.get_param('~default_vw_max', value)
326  self._widget.max_z_angular_double_spin_box.setValue(float(value))
327 
328  value = self._widget.min_z_angular_double_spin_box.value()
329  value = instance_settings.value('vw_min', value)
330  value = rospy.get_param('~default_vw_min', value)
331  self._widget.min_z_angular_double_spin_box.setValue(float(value))
rqt_robot_steering.robot_steering.RobotSteering._on_strong_decrease_z_angular_pressed
def _on_strong_decrease_z_angular_pressed(self)
Definition: robot_steering.py:258
rqt_robot_steering.robot_steering.RobotSteering._publisher
_publisher
Definition: robot_steering.py:52
rqt_robot_steering.robot_steering.RobotSteering._on_max_z_angular_changed
def _on_max_z_angular_changed(self, value)
Definition: robot_steering.py:238
rqt_robot_steering.robot_steering.RobotSteering._send_twist
def _send_twist(self, x_linear, z_angular)
Definition: robot_steering.py:267
rqt_robot_steering.robot_steering.RobotSteering.shortcut_shift_z
shortcut_shift_z
Definition: robot_steering.py:135
rqt_robot_steering.robot_steering.RobotSteering.shortcut_s
shortcut_s
Definition: robot_steering.py:102
rqt_robot_steering.robot_steering.RobotSteering._on_z_angular_slider_changed
def _on_z_angular_slider_changed(self)
Definition: robot_steering.py:203
rqt_robot_steering.robot_steering.RobotSteering.restore_settings
def restore_settings(self, plugin_settings, instance_settings)
Definition: robot_steering.py:308
rqt_robot_steering.robot_steering.RobotSteering._on_min_x_linear_changed
def _on_min_x_linear_changed(self, value)
Definition: robot_steering.py:234
rqt_gui_py::plugin::Plugin
rqt_robot_steering.robot_steering.RobotSteering._unregister_publisher
def _unregister_publisher(self)
Definition: robot_steering.py:287
rqt_robot_steering.robot_steering.RobotSteering.shortcut_d
shortcut_d
Definition: robot_steering.py:111
rqt_robot_steering.robot_steering.RobotSteering._on_decrease_x_linear_pressed
def _on_decrease_x_linear_pressed(self)
Definition: robot_steering.py:215
rqt_robot_steering.robot_steering.RobotSteering.shutdown_plugin
def shutdown_plugin(self)
Definition: robot_steering.py:292
rqt_robot_steering.robot_steering.RobotSteering.shortcut_shift_a
shortcut_shift_a
Definition: robot_steering.py:130
rqt_robot_steering.robot_steering.RobotSteering.shortcut_shift_s
shortcut_shift_s
Definition: robot_steering.py:125
rqt_robot_steering.robot_steering.RobotSteering.save_settings
def save_settings(self, plugin_settings, instance_settings)
Definition: robot_steering.py:296
rqt_robot_steering.robot_steering.RobotSteering._on_decrease_z_angular_pressed
def _on_decrease_z_angular_pressed(self)
Definition: robot_steering.py:226
rqt_robot_steering.robot_steering.RobotSteering._on_reset_z_angular_pressed
def _on_reset_z_angular_pressed(self)
Definition: robot_steering.py:223
rqt_robot_steering.robot_steering.RobotSteering._on_x_linear_slider_changed
def _on_x_linear_slider_changed(self)
Definition: robot_steering.py:198
rqt_robot_steering.robot_steering.RobotSteering.__init__
def __init__(self, context)
Definition: robot_steering.py:48
rqt_robot_steering.robot_steering.RobotSteering._on_topic_changed
def _on_topic_changed(self, topic)
Definition: robot_steering.py:178
rqt_robot_steering.robot_steering.RobotSteering.shortcut_shift_w
shortcut_shift_w
Definition: robot_steering.py:115
rqt_robot_steering.robot_steering.RobotSteering.shortcut_z
shortcut_z
Definition: robot_steering.py:108
rqt_robot_steering.robot_steering.RobotSteering._on_stop_pressed
def _on_stop_pressed(self)
Definition: robot_steering.py:188
rqt_robot_steering.robot_steering.RobotSteering.shortcut_space
shortcut_space
Definition: robot_steering.py:146
rqt_robot_steering.robot_steering.RobotSteering
Definition: robot_steering.py:44
rqt_robot_steering.robot_steering.RobotSteering._on_strong_increase_z_angular_pressed
def _on_strong_increase_z_angular_pressed(self)
Definition: robot_steering.py:254
rqt_robot_steering.robot_steering.RobotSteering._update_parameter_timer
_update_parameter_timer
Definition: robot_steering.py:171
rqt_robot_steering.robot_steering.RobotSteering._on_increase_x_linear_pressed
def _on_increase_x_linear_pressed(self)
Definition: robot_steering.py:208
rqt_robot_steering.robot_steering.RobotSteering.shortcut_a
shortcut_a
Definition: robot_steering.py:105
rqt_robot_steering.robot_steering.RobotSteering.shortcut_w
shortcut_w
Definition: robot_steering.py:96
rqt_robot_steering.robot_steering.RobotSteering._on_max_x_linear_changed
def _on_max_x_linear_changed(self, value)
Definition: robot_steering.py:230
rqt_robot_steering.robot_steering.RobotSteering._on_increase_z_angular_pressed
def _on_increase_z_angular_pressed(self)
Definition: robot_steering.py:219
rqt_robot_steering.robot_steering.RobotSteering.shortcut_x
shortcut_x
Definition: robot_steering.py:99
rqt_robot_steering.robot_steering.RobotSteering.shortcut_shift_d
shortcut_shift_d
Definition: robot_steering.py:140
rqt_robot_steering.robot_steering.RobotSteering._on_min_z_angular_changed
def _on_min_z_angular_changed(self, value)
Definition: robot_steering.py:242
rqt_robot_steering.robot_steering.RobotSteering.zero_cmd_sent
zero_cmd_sent
Definition: robot_steering.py:175
rqt_robot_steering.robot_steering.RobotSteering._on_reset_x_linear_pressed
def _on_reset_x_linear_pressed(self)
Definition: robot_steering.py:212
rqt_robot_steering.robot_steering.RobotSteering._on_parameter_changed
def _on_parameter_changed(self)
Definition: robot_steering.py:262
rqt_robot_steering.robot_steering.RobotSteering._on_strong_decrease_x_linear_pressed
def _on_strong_decrease_x_linear_pressed(self)
Definition: robot_steering.py:250
rqt_gui_py::plugin
rqt_robot_steering.robot_steering.RobotSteering._on_strong_increase_x_linear_pressed
def _on_strong_increase_x_linear_pressed(self)
Definition: robot_steering.py:246
rqt_robot_steering.robot_steering.RobotSteering._widget
_widget
Definition: robot_steering.py:54
rqt_robot_steering.robot_steering.RobotSteering.shortcut_shift_x
shortcut_shift_x
Definition: robot_steering.py:120


rqt_robot_steering
Author(s): Dirk Thomas
autogenerated on Fri Oct 6 2023 02:34:36