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