rqt_robot_steering: robot_steering.py Source File

Go to the documentation of this file.
00001 # Copyright (c) 2011, Dirk Thomas, TU Darmstadt
00002 # All rights reserved.
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions
00006 # are met:
00007 #
00008 #   * Redistributions of source code must retain the above copyright
00009 #     notice, this list of conditions and the following disclaimer.
00010 #   * Redistributions in binary form must reproduce the above
00011 #     copyright notice, this list of conditions and the following
00012 #     disclaimer in the documentation and/or other materials provided
00013 #     with the distribution.
00014 #   * Neither the name of the TU Darmstadt nor the names of its
00015 #     contributors may be used to endorse or promote products derived
00016 #     from this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00019 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00020 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00021 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00022 # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00023 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00024 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00027 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00028 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 # POSSIBILITY OF SUCH DAMAGE.
00030 
00031 from __future__ import division
00032 import os
00033 import rospkg
00034 
00035 from geometry_msgs.msg import Twist
00036 import rospy
00037 from python_qt_binding import loadUi
00038 from python_qt_binding.QtCore import Qt, QTimer, Slot
00039 from python_qt_binding.QtGui import QKeySequence
00040 from python_qt_binding.QtWidgets import QShortcut, QWidget
00041 from rqt_gui_py.plugin import Plugin
00042 
00043 
00044 class RobotSteering(Plugin):
00045 
00046     slider_factor = 1000.0
00047 
00048     def __init__(self, context):
00049         super(RobotSteering, self).__init__(context)
00050         self.setObjectName('RobotSteering')
00051 
00052         self._publisher = None
00053 
00054         self._widget = QWidget()
00055         rp = rospkg.RosPack()
00056         ui_file = os.path.join(
00057             rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
00058         loadUi(ui_file, self._widget)
00059         self._widget.setObjectName('RobotSteeringUi')
00060         if context.serial_number() > 1:
00061             self._widget.setWindowTitle(
00062                 self._widget.windowTitle() + (' (%d)' % context.serial_number()))
00063         context.add_widget(self._widget)
00064 
00065         self._widget.topic_line_edit.textChanged.connect(
00066             self._on_topic_changed)
00067         self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
00068 
00069         self._widget.x_linear_slider.valueChanged.connect(
00070             self._on_x_linear_slider_changed)
00071         self._widget.z_angular_slider.valueChanged.connect(
00072             self._on_z_angular_slider_changed)
00073 
00074         self._widget.increase_x_linear_push_button.pressed.connect(
00075             self._on_strong_increase_x_linear_pressed)
00076         self._widget.reset_x_linear_push_button.pressed.connect(
00077             self._on_reset_x_linear_pressed)
00078         self._widget.decrease_x_linear_push_button.pressed.connect(
00079             self._on_strong_decrease_x_linear_pressed)
00080         self._widget.increase_z_angular_push_button.pressed.connect(
00081             self._on_strong_increase_z_angular_pressed)
00082         self._widget.reset_z_angular_push_button.pressed.connect(
00083             self._on_reset_z_angular_pressed)
00084         self._widget.decrease_z_angular_push_button.pressed.connect(
00085             self._on_strong_decrease_z_angular_pressed)
00086 
00087         self._widget.max_x_linear_double_spin_box.valueChanged.connect(
00088             self._on_max_x_linear_changed)
00089         self._widget.min_x_linear_double_spin_box.valueChanged.connect(
00090             self._on_min_x_linear_changed)
00091         self._widget.max_z_angular_double_spin_box.valueChanged.connect(
00092             self._on_max_z_angular_changed)
00093         self._widget.min_z_angular_double_spin_box.valueChanged.connect(
00094             self._on_min_z_angular_changed)
00095 
00096         self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
00097         self.shortcut_w.setContext(Qt.ApplicationShortcut)
00098         self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
00099         self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
00100         self.shortcut_x.setContext(Qt.ApplicationShortcut)
00101         self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
00102         self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
00103         self.shortcut_s.setContext(Qt.ApplicationShortcut)
00104         self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
00105         self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
00106         self.shortcut_a.setContext(Qt.ApplicationShortcut)
00107         self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
00108         self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
00109         self.shortcut_z.setContext(Qt.ApplicationShortcut)
00110         self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
00111         self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
00112         self.shortcut_d.setContext(Qt.ApplicationShortcut)
00113         self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
00114 
00115         self.shortcut_shift_w = QShortcut(
00116             QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
00117         self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
00118         self.shortcut_shift_w.activated.connect(
00119             self._on_strong_increase_x_linear_pressed)
00120         self.shortcut_shift_x = QShortcut(
00121             QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
00122         self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
00123         self.shortcut_shift_x.activated.connect(
00124             self._on_reset_x_linear_pressed)
00125         self.shortcut_shift_s = QShortcut(
00126             QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
00127         self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
00128         self.shortcut_shift_s.activated.connect(
00129             self._on_strong_decrease_x_linear_pressed)
00130         self.shortcut_shift_a = QShortcut(
00131             QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
00132         self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
00133         self.shortcut_shift_a.activated.connect(
00134             self._on_strong_increase_z_angular_pressed)
00135         self.shortcut_shift_z = QShortcut(
00136             QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
00137         self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
00138         self.shortcut_shift_z.activated.connect(
00139             self._on_reset_z_angular_pressed)
00140         self.shortcut_shift_d = QShortcut(
00141             QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
00142         self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
00143         self.shortcut_shift_d.activated.connect(
00144             self._on_strong_decrease_z_angular_pressed)
00145 
00146         self.shortcut_space = QShortcut(
00147             QKeySequence(Qt.Key_Space), self._widget)
00148         self.shortcut_space.setContext(Qt.ApplicationShortcut)
00149         self.shortcut_space.activated.connect(self._on_stop_pressed)
00150         self.shortcut_space = QShortcut(
00151             QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
00152         self.shortcut_space.setContext(Qt.ApplicationShortcut)
00153         self.shortcut_space.activated.connect(self._on_stop_pressed)
00154 
00155         self._widget.stop_push_button.setToolTip(
00156             self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
00157         self._widget.increase_x_linear_push_button.setToolTip(
00158             self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
00159         self._widget.reset_x_linear_push_button.setToolTip(
00160             self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
00161         self._widget.decrease_x_linear_push_button.setToolTip(
00162             self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
00163         self._widget.increase_z_angular_push_button.setToolTip(
00164             self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
00165         self._widget.reset_z_angular_push_button.setToolTip(
00166             self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
00167         self._widget.decrease_z_angular_push_button.setToolTip(
00168             self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
00169 
00170         # timer to consecutively send twist messages
00171         self._update_parameter_timer = QTimer(self)
00172         self._update_parameter_timer.timeout.connect(
00173             self._on_parameter_changed)
00174         self._update_parameter_timer.start(100)
00175         self.zero_cmd_sent = False
00176 
00177     @Slot(str)
00178     def _on_topic_changed(self, topic):
00179         topic = str(topic)
00180         self._unregister_publisher()
00181         if topic == '':
00182             return
00183         try:
00184             self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
00185         except TypeError:
00186             self._publisher = rospy.Publisher(topic, Twist)
00187 
00188     def _on_stop_pressed(self):
00189         # If the current value of sliders is zero directly send stop twist msg
00190         if self._widget.x_linear_slider.value() is 0 and \
00191                 self._widget.z_angular_slider.value() is 0:
00192             self.zero_cmd_sent = False
00193             self._on_parameter_changed()
00194         else:
00195             self._widget.x_linear_slider.setValue(0)
00196             self._widget.z_angular_slider.setValue(0)
00197 
00198     def _on_x_linear_slider_changed(self):
00199         self._widget.current_x_linear_label.setText(
00200             '%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor))
00201         self._on_parameter_changed()
00202 
00203     def _on_z_angular_slider_changed(self):
00204         self._widget.current_z_angular_label.setText(
00205             '%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor))
00206         self._on_parameter_changed()
00207 
00208     def _on_increase_x_linear_pressed(self):
00209         self._widget.x_linear_slider.setValue(
00210             self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
00211 
00212     def _on_reset_x_linear_pressed(self):
00213         self._widget.x_linear_slider.setValue(0)
00214 
00215     def _on_decrease_x_linear_pressed(self):
00216         self._widget.x_linear_slider.setValue(
00217             self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
00218 
00219     def _on_increase_z_angular_pressed(self):
00220         self._widget.z_angular_slider.setValue(
00221             self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
00222 
00223     def _on_reset_z_angular_pressed(self):
00224         self._widget.z_angular_slider.setValue(0)
00225 
00226     def _on_decrease_z_angular_pressed(self):
00227         self._widget.z_angular_slider.setValue(
00228             self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
00229 
00230     def _on_max_x_linear_changed(self, value):
00231         self._widget.x_linear_slider.setMaximum(
00232             value * RobotSteering.slider_factor)
00233 
00234     def _on_min_x_linear_changed(self, value):
00235         self._widget.x_linear_slider.setMinimum(
00236             value * RobotSteering.slider_factor)
00237 
00238     def _on_max_z_angular_changed(self, value):
00239         self._widget.z_angular_slider.setMaximum(
00240             value * RobotSteering.slider_factor)
00241 
00242     def _on_min_z_angular_changed(self, value):
00243         self._widget.z_angular_slider.setMinimum(
00244             value * RobotSteering.slider_factor)
00245 
00246     def _on_strong_increase_x_linear_pressed(self):
00247         self._widget.x_linear_slider.setValue(
00248             self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
00249 
00250     def _on_strong_decrease_x_linear_pressed(self):
00251         self._widget.x_linear_slider.setValue(
00252             self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
00253 
00254     def _on_strong_increase_z_angular_pressed(self):
00255         self._widget.z_angular_slider.setValue(
00256             self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
00257 
00258     def _on_strong_decrease_z_angular_pressed(self):
00259         self._widget.z_angular_slider.setValue(
00260             self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
00261 
00262     def _on_parameter_changed(self):
00263         self._send_twist(
00264             self._widget.x_linear_slider.value() / RobotSteering.slider_factor,
00265                          self._widget.z_angular_slider.value() / RobotSteering.slider_factor)
00266 
00267     def _send_twist(self, x_linear, z_angular):
00268         if self._publisher is None:
00269             return
00270         twist = Twist()
00271         twist.linear.x = x_linear
00272         twist.linear.y = 0
00273         twist.linear.z = 0
00274         twist.angular.x = 0
00275         twist.angular.y = 0
00276         twist.angular.z = z_angular
00277 
00278         # Only send the zero command once so other devices can take control
00279         if x_linear == 0 and z_angular == 0:
00280             if not self.zero_cmd_sent:
00281                 self.zero_cmd_sent = True
00282                 self._publisher.publish(twist)
00283         else:
00284             self.zero_cmd_sent = False
00285             self._publisher.publish(twist)
00286 
00287     def _unregister_publisher(self):
00288         if self._publisher is not None:
00289             self._publisher.unregister()
00290             self._publisher = None
00291 
00292     def shutdown_plugin(self):
00293         self._update_parameter_timer.stop()
00294         self._unregister_publisher()
00295 
00296     def save_settings(self, plugin_settings, instance_settings):
00297         instance_settings.set_value(
00298             'topic', self._widget.topic_line_edit.text())
00299         instance_settings.set_value(
00300             'vx_max', self._widget.max_x_linear_double_spin_box.value())
00301         instance_settings.set_value(
00302             'vx_min', self._widget.min_x_linear_double_spin_box.value())
00303         instance_settings.set_value(
00304             'vw_max', self._widget.max_z_angular_double_spin_box.value())
00305         instance_settings.set_value(
00306             'vw_min', self._widget.min_z_angular_double_spin_box.value())
00307 
00308     def restore_settings(self, plugin_settings, instance_settings):
00309         value = instance_settings.value('topic', "/cmd_vel")
00310         value = rospy.get_param("~default_topic", value)
00311         self._widget.topic_line_edit.setText(value)
00312 
00313         value = self._widget.max_x_linear_double_spin_box.value()
00314         value = instance_settings.value('vx_max', value)
00315         value = rospy.get_param("~default_vx_max", value)
00316         self._widget.max_x_linear_double_spin_box.setValue(float(value))
00317 
00318         value = self._widget.min_x_linear_double_spin_box.value()
00319         value = instance_settings.value('vx_min', value)
00320         value = rospy.get_param("~default_vx_min", value)
00321         self._widget.min_x_linear_double_spin_box.setValue(float(value))
00322 
00323         value = self._widget.max_z_angular_double_spin_box.value()
00324         value = instance_settings.value('vw_max', value)
00325         value = rospy.get_param("~default_vw_max", value)
00326         self._widget.max_z_angular_double_spin_box.setValue(float(value))
00327 
00328         value = self._widget.min_z_angular_double_spin_box.value()
00329         value = instance_settings.value('vw_min', value)
00330         value = rospy.get_param("~default_vw_min", value)
00331         self._widget.min_z_angular_double_spin_box.setValue(float(value))