Go to the documentation of this file.00001
00002
00003 """
00004
00005 Copyright (C) 2012 Jon Stephan.
00006
00007 This program is free software: you can redistribute it and/or modify
00008 it under the terms of the GNU General Public License as published by
00009 the Free Software Foundation, either version 3 of the License, or
00010 (at your option) any later version.
00011
00012 This program is distributed in the hope that it will be useful,
00013 but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00015 GNU General Public License for more details.
00016
00017 You should have received a copy of the GNU General Public License
00018 along with this program. If not, see <http://www.gnu.org/licenses/>.
00019
00020 """
00021 import sys
00022
00023 import roslib; roslib.load_manifest('differential_drive')
00024 import rospy
00025 from geometry_msgs.msg import Twist
00026 import inspect, os
00027
00028
00029 from PySide import QtGui, QtCore
00030
00031
00032
00033 class MainWindow(QtGui.QMainWindow):
00034
00035
00036
00037
00038 def __init__(self):
00039
00040 super(MainWindow, self).__init__()
00041 self.timer_rate = rospy.get_param('~publish_rate', 50)
00042 self.pub_twist = rospy.Publisher('twist', Twist)
00043
00044 self.initUI()
00045
00046
00047 def initUI(self):
00048
00049
00050 img_path = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) + "/../images/crosshair.jpg"
00051 rospy.loginfo("initUI img_path: %s" % img_path)
00052
00053 self.statusBar()
00054
00055 self.setStyleSheet("QMainWindow { border-image: url(%s); }" % img_path)
00056
00057
00058 self.setGeometry(0, 600, 200, 200)
00059 self.setWindowTitle('Virtual Joystick')
00060 self.show()
00061 self.timer = QtCore.QBasicTimer()
00062
00063 self.statusBar().showMessage('started')
00064
00065
00066 def mousePressEvent(self, event):
00067
00068 self.statusBar().showMessage('mouse clicked')
00069 self.timer.start(self.timer_rate, self)
00070 self.get_position(event)
00071
00072
00073 def mouseReleaseEvent(self, event):
00074
00075 self.statusBar().showMessage('mouse released')
00076 self.timer.stop()
00077
00078
00079 def mouseMoveEvent(self, event):
00080
00081 self.get_position(event)
00082
00083
00084 def get_position(self, event):
00085
00086 s = self.size()
00087 s_w = s.width()
00088 s_h = s.height()
00089 pos = event.pos()
00090 self.x = 1.0 * pos.x() / s_w
00091 self.y = 1.0 * pos.y() / s_h
00092
00093 self.statusBar().showMessage('point (%0.2f, %0.2f)' % (self.x,self.y))
00094
00095
00096 def timerEvent(self, event):
00097
00098
00099 self.pubTwist()
00100
00101
00102 def pubTwist(self):
00103
00104
00105 self.twist = Twist()
00106 self.twist.linear.x = (1-self.y) * (x_max - x_min) + x_min
00107 self.twist.linear.y = 0
00108 self.twist.linear.z = 0
00109 self.twist.angular.x = 0
00110 self.twist.angular.y = 0
00111 self.twist.angular.z = (1-self.x) * (r_max - r_min) + r_min
00112
00113 if self.twist.linear.x > x_max:
00114 self.twist.linear.x = x_max
00115 if self.twist.linear.x < x_min:
00116 self.twist.linear.x = x_min
00117 if self.twist.angular.z > r_max:
00118 self.twist.angular.z = r_max
00119 if self.twist.angular.z < r_min:
00120 self.twist.angular.z = r_min
00121
00122 self.pub_twist.publish( self.twist )
00123
00124
00125
00126 def main():
00127
00128
00129 rospy.init_node('virtual_joystick')
00130 rospy.loginfo('virtual_joystick started')
00131 global x_min
00132 global x_max
00133 global r_min
00134 global r_max
00135
00136 x_min = rospy.get_param("~x_min", -0.20)
00137 x_max = rospy.get_param("~x_max", 0.20)
00138 r_min = rospy.get_param("~r_min", -1.0)
00139 r_max = rospy.get_param("~r_max", 1.0)
00140
00141 app = QtGui.QApplication(sys.argv)
00142 ex = MainWindow()
00143 sys.exit(app.exec_())
00144
00145 if __name__ == '__main__':
00146 try:
00147 main()
00148 except rospy.ROSInterruptException: pass