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 
00034 from python_qt_binding import loadUi
00035 from python_qt_binding.QtCore import Qt, QTimer, Slot
00036 from python_qt_binding.QtGui import QKeySequence, QShortcut, QWidget
00037 
00038 import roslib
00039 roslib.load_manifest('rqt_robot_steering')
00040 import rospy
00041 
00042 from geometry_msgs.msg import Twist
00043 
00044 from rqt_gui_py.plugin import Plugin
00045 
00046 
00047 class RobotSteering(Plugin):
00048 
00049     def __init__(self, context):
00050         super(RobotSteering, self).__init__(context)
00051         self.setObjectName('RobotSteering')
00052 
00053         self._publisher = None
00054 
00055         self._widget = QWidget()
00056         ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '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 
00065         self._widget.x_linear_slider.valueChanged.connect(self._on_parameter_changed)
00066         self._widget.z_angular_slider.valueChanged.connect(self._on_parameter_changed)
00067 
00068         self._widget.increase_x_linear_push_button.pressed.connect(self._on_increase_x_linear_pressed)
00069         self._widget.decrease_x_linear_push_button.pressed.connect(self._on_decrease_x_linear_pressed)
00070         self._widget.increase_z_angular_push_button.pressed.connect(self._on_increase_z_angular_pressed)
00071         self._widget.decrease_z_angular_push_button.pressed.connect(self._on_decrease_z_angular_pressed)
00072         self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
00073 
00074         self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
00075         self.shortcut_w.setContext(Qt.ApplicationShortcut)
00076         self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
00077         self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
00078         self.shortcut_s.setContext(Qt.ApplicationShortcut)
00079         self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
00080         self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
00081         self.shortcut_a.setContext(Qt.ApplicationShortcut)
00082         self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
00083         self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
00084         self.shortcut_d.setContext(Qt.ApplicationShortcut)
00085         self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
00086 
00087         self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
00088         self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
00089         self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
00090         self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
00091         self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
00092         self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
00093         self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
00094         self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
00095         self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
00096         self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
00097         self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
00098         self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
00099 
00100         self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
00101         self.shortcut_space.setContext(Qt.ApplicationShortcut)
00102         self.shortcut_space.activated.connect(self._on_stop_pressed)
00103 
00104         # timer to consecutively send twist messages
00105         self._update_parameter_timer = QTimer(self)
00106         self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
00107         self._update_parameter_timer.start(100)
00108 
00109     @Slot(str)
00110     def _on_topic_changed(self, topic):
00111         topic = str(topic)
00112         self._unregister_publisher()
00113         self._publisher = rospy.Publisher(topic, Twist)
00114 
00115     def _on_parameter_changed(self):
00116         self._send_twist(self._widget.x_linear_slider.value() / 1000.0, self._widget.z_angular_slider.value() / 1000.0)
00117 
00118     def _send_twist(self, x_linear, z_angular):
00119         if self._publisher is None:
00120             return
00121         twist = Twist()
00122         twist.linear.x = x_linear
00123         twist.linear.y = 0
00124         twist.linear.z = 0
00125         twist.angular.x = 0
00126         twist.angular.y = 0
00127         twist.angular.z = z_angular
00128         self._publisher.publish(twist)
00129 
00130     def _on_increase_x_linear_pressed(self):
00131         self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
00132 
00133     def _on_decrease_x_linear_pressed(self):
00134         self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
00135 
00136     def _on_increase_z_angular_pressed(self):
00137         self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
00138 
00139     def _on_decrease_z_angular_pressed(self):
00140         self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
00141 
00142     def _on_strong_increase_x_linear_pressed(self):
00143         self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
00144 
00145     def _on_strong_decrease_x_linear_pressed(self):
00146         self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
00147 
00148     def _on_strong_increase_z_angular_pressed(self):
00149         self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
00150 
00151     def _on_strong_decrease_z_angular_pressed(self):
00152         self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
00153 
00154     def _on_stop_pressed(self):
00155         self._widget.x_linear_slider.setValue(0)
00156         self._widget.z_angular_slider.setValue(0)
00157 
00158     def _unregister_publisher(self):
00159         if self._publisher is not None:
00160             self._publisher.unregister()
00161             self._publisher = None
00162 
00163     def shutdown_plugin(self):
00164         self._update_parameter_timer.stop()
00165         self._unregister_publisher()
00166 
00167     def save_settings(self, plugin_settings, instance_settings):
00168         topic = self._widget.topic_line_edit.text()
00169         instance_settings.set_value('topic', topic)
00170 
00171     def restore_settings(self, plugin_settings, instance_settings):
00172         topic = instance_settings.value('topic', '/cmd_vel')
00173         self._widget.topic_line_edit.setText(topic)