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, QShortcut, QWidget
00040 from rqt_gui_py.plugin import Plugin
00041 
00042 
00043 class RobotSteering(Plugin):
00044 
00045     slider_factor = 1000.0
00046 
00047     def __init__(self, context):
00048         super(RobotSteering, self).__init__(context)
00049         self.setObjectName('RobotSteering')
00050 
00051         self._publisher = None
00052 
00053         self._widget = QWidget()
00054         rp = rospkg.RosPack()
00055         ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
00056         loadUi(ui_file, self._widget)
00057         self._widget.setObjectName('RobotSteeringUi')
00058         if context.serial_number() > 1:
00059             self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
00060         context.add_widget(self._widget)
00061 
00062         self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
00063         self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
00064 
00065         self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
00066         self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
00067 
00068         self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
00069         self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
00070         self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
00071         self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
00072         self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
00073         self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
00074 
00075         self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
00076         self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
00077         self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
00078         self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
00079 
00080         self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
00081         self.shortcut_w.setContext(Qt.ApplicationShortcut)
00082         self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
00083         self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
00084         self.shortcut_x.setContext(Qt.ApplicationShortcut)
00085         self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
00086         self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
00087         self.shortcut_s.setContext(Qt.ApplicationShortcut)
00088         self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
00089         self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
00090         self.shortcut_a.setContext(Qt.ApplicationShortcut)
00091         self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
00092         self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
00093         self.shortcut_z.setContext(Qt.ApplicationShortcut)
00094         self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
00095         self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
00096         self.shortcut_d.setContext(Qt.ApplicationShortcut)
00097         self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
00098 
00099         self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
00100         self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
00101         self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
00102         self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
00103         self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
00104         self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
00105         self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
00106         self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
00107         self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
00108         self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
00109         self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
00110         self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
00111         self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
00112         self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
00113         self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
00114         self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
00115         self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
00116         self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
00117 
00118         self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
00119         self.shortcut_space.setContext(Qt.ApplicationShortcut)
00120         self.shortcut_space.activated.connect(self._on_stop_pressed)
00121         self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
00122         self.shortcut_space.setContext(Qt.ApplicationShortcut)
00123         self.shortcut_space.activated.connect(self._on_stop_pressed)
00124 
00125         self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
00126         self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
00127         self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
00128         self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
00129         self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
00130         self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
00131         self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
00132 
00133         # timer to consecutively send twist messages
00134         self._update_parameter_timer = QTimer(self)
00135         self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
00136         self._update_parameter_timer.start(100)
00137         self.zero_cmd_sent = False
00138 
00139     @Slot(str)
00140     def _on_topic_changed(self, topic):
00141         topic = str(topic)
00142         self._unregister_publisher()
00143         try:
00144             self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
00145         except TypeError:
00146             self._publisher = rospy.Publisher(topic, Twist)
00147 
00148     def _on_stop_pressed(self):
00149         self._widget.x_linear_slider.setValue(0)
00150         self._widget.z_angular_slider.setValue(0)
00151 
00152     def _on_x_linear_slider_changed(self):
00153         self._widget.current_x_linear_label.setText('%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor))
00154         self._on_parameter_changed()
00155 
00156     def _on_z_angular_slider_changed(self):
00157         self._widget.current_z_angular_label.setText('%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor))
00158         self._on_parameter_changed()
00159 
00160     def _on_increase_x_linear_pressed(self):
00161         self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
00162 
00163     def _on_reset_x_linear_pressed(self):
00164         self._widget.x_linear_slider.setValue(0)
00165 
00166     def _on_decrease_x_linear_pressed(self):
00167         self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
00168 
00169     def _on_increase_z_angular_pressed(self):
00170         self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
00171 
00172     def _on_reset_z_angular_pressed(self):
00173         self._widget.z_angular_slider.setValue(0)
00174 
00175     def _on_decrease_z_angular_pressed(self):
00176         self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
00177 
00178     def _on_max_x_linear_changed(self, value):
00179         self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor)
00180 
00181     def _on_min_x_linear_changed(self, value):
00182         self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor)
00183 
00184     def _on_max_z_angular_changed(self, value):
00185         self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor)
00186 
00187     def _on_min_z_angular_changed(self, value):
00188         self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor)
00189 
00190     def _on_strong_increase_x_linear_pressed(self):
00191         self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
00192 
00193     def _on_strong_decrease_x_linear_pressed(self):
00194         self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
00195 
00196     def _on_strong_increase_z_angular_pressed(self):
00197         self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
00198 
00199     def _on_strong_decrease_z_angular_pressed(self):
00200         self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
00201 
00202     def _on_parameter_changed(self):
00203         self._send_twist(self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor)
00204 
00205     def _send_twist(self, x_linear, z_angular):
00206         if self._publisher is None:
00207             return
00208         twist = Twist()
00209         twist.linear.x = x_linear
00210         twist.linear.y = 0
00211         twist.linear.z = 0
00212         twist.angular.x = 0
00213         twist.angular.y = 0
00214         twist.angular.z = z_angular
00215 
00216         # Only send the zero command once so other devices can take control
00217         if x_linear == 0 and z_angular == 0:
00218             if not self.zero_cmd_sent:
00219                 self.zero_cmd_sent = True
00220                 self._publisher.publish(twist)
00221         else:
00222             self.zero_cmd_sent = False
00223             self._publisher.publish(twist)
00224 
00225     def _unregister_publisher(self):
00226         if self._publisher is not None:
00227             self._publisher.unregister()
00228             self._publisher = None
00229 
00230     def shutdown_plugin(self):
00231         self._update_parameter_timer.stop()
00232         self._unregister_publisher()
00233 
00234     def save_settings(self, plugin_settings, instance_settings):
00235         instance_settings.set_value('topic' , self._widget.topic_line_edit.text())
00236         instance_settings.set_value('vx_max', self._widget.max_x_linear_double_spin_box.value())
00237         instance_settings.set_value('vx_min', self._widget.min_x_linear_double_spin_box.value()) 
00238         instance_settings.set_value('vw_max', self._widget.max_z_angular_double_spin_box.value())
00239         instance_settings.set_value('vw_min', self._widget.min_z_angular_double_spin_box.value())
00240         
00241     def restore_settings(self, plugin_settings, instance_settings):
00242                      
00243         value = instance_settings.value('topic', "/cmd_vel")
00244         value = rospy.get_param("~default_topic", value)           
00245         self._widget.topic_line_edit.setText(value)
00246         
00247         value = self._widget.max_x_linear_double_spin_box.value()
00248         value = instance_settings.value( 'vx_max', value)
00249         value = rospy.get_param("~default_vx_max", value)           
00250         self._widget.max_x_linear_double_spin_box.setValue(float(value))
00251         
00252         value = self._widget.min_x_linear_double_spin_box.value()
00253         value = instance_settings.value( 'vx_min', value)
00254         value = rospy.get_param("~default_vx_min", value)    
00255         self._widget.min_x_linear_double_spin_box.setValue(float(value))
00256         
00257         value = self._widget.max_z_angular_double_spin_box.value()
00258         value = instance_settings.value( 'vw_max', value)
00259         value = rospy.get_param("~default_vw_max", value)     
00260         self._widget.max_z_angular_double_spin_box.setValue(float(value))
00261         
00262         value = self._widget.min_z_angular_double_spin_box.value()
00263         value = instance_settings.value( 'vw_min', value)
00264         value = rospy.get_param("~default_vw_min", value) 
00265         self._widget.min_z_angular_double_spin_box.setValue(float(value))
00266         
00267         
00268         
00269         
00270         
00271         
00272         
00273